(* Content-type: application/mathematica *)

(*** Wolfram Notebook File ***)
(* http://www.wolfram.com/nb *)

(* CreatedBy='Mathematica 7.0' *)

(*CacheID: 234*)
(* Internal cache information:
NotebookFileLineBreakTest
NotebookFileLineBreakTest
NotebookDataPosition[       145,          7]
NotebookDataLength[    273888,       7390]
NotebookOptionsPosition[    271726,       7321]
NotebookOutlinePosition[    272119,       7338]
CellTagsIndexPosition[    272076,       7335]
WindowFrame->Normal*)

(* Beginning of Notebook Content *)
Notebook[{

Cell[CellGroupData[{
Cell[BoxData[
 RowBox[{
  RowBox[{"(*", "AMBER", "*)"}], "\[IndentingNewLine]", 
  RowBox[{
   RowBox[{
    RowBox[{
    "SetDirectory", "[", 
     "\"\</home/shu/workspace/Research/AMBER_Output_Test/model\>\"", "]"}], 
    ";"}], "\n", 
   RowBox[{"<<", "RobotLinks.m"}], "\n", 
   RowBox[{"<<", "Linearize.m"}], "\n", 
   RowBox[{"(*", 
    RowBox[{
     RowBox[{"SetDirectory", "[", 
      RowBox[{
      "\"\</home/shu/workspace/Research/AMBER_Output_Test/model/\>\"", "<>", 
       "\"\<build_torso\>\""}], "]"}], ";"}], "*)"}], "\n", 
   RowBox[{
    RowBox[{"LineNumber", " ", "=", " ", "1"}], ";"}], "\n", 
   RowBox[{
    RowBox[{"IndexAllPre", " ", "=", " ", 
     RowBox[{"Import", "[", 
      RowBox[{"\"\<../data/IndexAll.mat\>\"", ",", "\"\<MAT\>\""}], "]"}]}], 
    ";"}], "\[IndentingNewLine]", 
   RowBox[{"(*", 
    RowBox[{
     RowBox[{"Dimensions", "[", "IndexAllPre", "]"}], ";"}], "*)"}], 
   "\[IndentingNewLine]", 
   RowBox[{
    RowBox[{"IndexAll", "=", 
     RowBox[{"Join", "[", 
      RowBox[{"First", "[", "IndexAllPre", "]"}], "]"}]}], ";"}], "\n", 
   RowBox[{"(*", 
    RowBox[{
     RowBox[{"Dimensions", "[", "IndexAll", "]"}], ";"}], "*)"}], 
   "\[IndentingNewLine]", "\[IndentingNewLine]", 
   RowBox[{"HipPosIndex", "=", 
    RowBox[{"Round", "[", 
     RowBox[{"IndexAll", "[", 
      RowBox[{"[", 
       RowBox[{"LineNumber", ",", "1"}], "]"}], "]"}], "]"}]}], 
   "\[IndentingNewLine]", 
   RowBox[{"NSslopeIndex", "=", 
    RowBox[{"Round", "[", 
     RowBox[{"IndexAll", "[", 
      RowBox[{"[", 
       RowBox[{"LineNumber", ",", "2"}], "]"}], "]"}], "]"}]}], 
   "\[IndentingNewLine]", 
   RowBox[{"TorsoIndex", "=", 
    RowBox[{"Round", "[", 
     RowBox[{"IndexAll", "[", 
      RowBox[{"[", 
       RowBox[{"LineNumber", ",", "3"}], "]"}], "]"}], "]"}]}]}]}]], "Input",
 CellChangeTimes->{
  3.504875918224949*^9, {3.505063952502133*^9, 3.505063953970308*^9}, {
   3.5051489336912956`*^9, 3.5051489340032964`*^9}, 3.513531757509935*^9, {
   3.515870024222504*^9, 3.5158700252255616`*^9}, 3.5158863755610266`*^9, {
   3.5271753048660097`*^9, 3.527175316328361*^9}, {3.5271784037700644`*^9, 
   3.527178409550562*^9}, {3.527194806620797*^9, 3.527194812727977*^9}, {
   3.527194901003693*^9, 3.527194939760748*^9}, 3.527195000990027*^9, 
   3.527195065536849*^9, {3.527197843268285*^9, 3.527197851612855*^9}, {
   3.527266876626479*^9, 3.5272669091021013`*^9}, 3.5272680582479362`*^9, {
   3.52726826837751*^9, 3.5272682685913153`*^9}, 3.527268323380795*^9, {
   3.5272685755855627`*^9, 3.527268578414917*^9}, {3.527278880328299*^9, 
   3.527278880502112*^9}}],

Cell[BoxData[
 FormBox["1", TraditionalForm]], "Output",
 CellChangeTimes->{3.527268578845785*^9, 3.527278884161552*^9, 
  3.5272803669441032`*^9, 3.5272819340319977`*^9}],

Cell[BoxData[
 FormBox["1", TraditionalForm]], "Output",
 CellChangeTimes->{3.527268578845785*^9, 3.527278884161552*^9, 
  3.5272803669441032`*^9, 3.527281934033134*^9}],

Cell[BoxData[
 FormBox["1", TraditionalForm]], "Output",
 CellChangeTimes->{3.527268578845785*^9, 3.527278884161552*^9, 
  3.5272803669441032`*^9, 3.527281934034472*^9}]
}, Open  ]],

Cell[BoxData[
 RowBox[{
  RowBox[{"(*", 
   RowBox[{
    RowBox[{"t0", "=", 
     RowBox[{"AbsoluteTime", "[", "]"}]}], ";"}], "*)"}], 
  "\[IndentingNewLine]", 
  RowBox[{"(*", 
   RowBox[{"model", " ", "specification"}], "*)"}], "\n", 
  RowBox[{
   RowBox[{
    RowBox[{"robotInfo", "=", 
     RowBox[{"{", 
      RowBox[{
       RowBox[{"{", 
        RowBox[{
        "Lc", ",", "Lt", ",", "LT", ",", "rc", ",", "rt", ",", "rT", ",", 
         "mh", ",", "mt", ",", "mc", ",", "g"}], "}"}], ",", 
       "\[IndentingNewLine]", 
       RowBox[{"{", 
        RowBox[{
        "347.98", ",", "261.112", ",", "0", ",", "282.37", ",", "128.15", ",",
          "9.97", ",", 
         StyleBox["804.83",
          FontWeight->"Plain"], ",", "606.15", ",", "213.79", ",", "9810"}], 
        "}"}]}], "}"}]}], ";"}], "\n", 
   RowBox[{
    RowBox[{"constsubs", "=", 
     RowBox[{
      RowBox[{"(", 
       RowBox[{
        RowBox[{
         RowBox[{"#1", "\[LeftDoubleBracket]", "1", "\[RightDoubleBracket]"}],
          "\[Rule]", 
         RowBox[{"Rationalize", "[", 
          FractionBox[
           RowBox[{
           "#1", "\[LeftDoubleBracket]", "2", "\[RightDoubleBracket]"}], 
           "1000"], "]"}]}], "&"}], ")"}], "/@", 
      RowBox[{"Transpose", "[", "robotInfo", "]"}]}]}], ";"}]}]}]], "Input",
 CellChangeTimes->{
  3.504875918224949*^9, {3.505063952502133*^9, 3.505063953970308*^9}, {
   3.5051489336912956`*^9, 3.5051489340032964`*^9}, 3.513531757509935*^9, {
   3.515870024222504*^9, 3.5158700252255616`*^9}, 3.5158863755610266`*^9, {
   3.5271753048660097`*^9, 3.527175316328361*^9}, {3.5271784037700644`*^9, 
   3.527178409550562*^9}, {3.527194806620797*^9, 3.527194812727977*^9}, {
   3.527194901003693*^9, 3.527194939760748*^9}, 3.527195000990027*^9, 
   3.527195065536849*^9, {3.527197843268285*^9, 3.527197851612855*^9}, {
   3.527266876626479*^9, 3.5272669091021013`*^9}, 3.5272680582479362`*^9, {
   3.52726826837751*^9, 3.5272682685913153`*^9}, {3.527268323380795*^9, 
   3.52726834320557*^9}, {3.527268721025804*^9, 3.5272687345158367`*^9}, 
   3.527271522279252*^9}],

Cell[BoxData[{
 RowBox[{
  RowBox[{"ndof", " ", "=", " ", "5"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"mm", "=", 
   RowBox[{
    RowBox[{"{", 
     RowBox[{"mc", ",", "mt", ",", "mh", ",", "mt", ",", "mc"}], "}"}], "/.", 
    "\[InvisibleSpace]", "constsubs"}]}], ";"}], "\n", 
 RowBox[{
  RowBox[{
   RowBox[{"statesubs", "=", 
    RowBox[{"Join", "[", 
     RowBox[{
      RowBox[{"Table", "[", 
       RowBox[{
        RowBox[{
         RowBox[{
          SubscriptBox["\[Theta]", "i"], "[", "t", "]"}], "\[Rule]", 
         RowBox[{"x", "[", "i", "]"}]}], ",", 
        RowBox[{"{", 
         RowBox[{"i", ",", "1", ",", "ndof"}], "}"}]}], "]"}], ",", 
      RowBox[{"Table", "[", 
       RowBox[{
        RowBox[{
         RowBox[{
          SuperscriptBox[
           SubscriptBox["\[Theta]", "i"], "\[Prime]",
           MultilineFunction->None], "[", "t", "]"}], "\[Rule]", 
         RowBox[{"x", "[", 
          RowBox[{"i", "+", "ndof"}], "]"}]}], ",", 
        RowBox[{"{", 
         RowBox[{"i", ",", "1", ",", "ndof"}], "}"}]}], "]"}]}], "]"}]}], 
   ";"}], "\[IndentingNewLine]", 
  RowBox[{"(*", 
   RowBox[{"define", " ", "inertia", " ", "matrices"}], "*)"}]}], "\n", 
 RowBox[{
  RowBox[{
   RowBox[{"For", "[", 
    RowBox[{
     RowBox[{"i", "=", "1"}], ",", 
     RowBox[{"i", "\[LessEqual]", 
      RowBox[{"Length", "[", "mm", "]"}]}], ",", 
     RowBox[{"i", "++"}], ",", 
     RowBox[{
      SubscriptBox["\[ScriptCapitalM]", "i"], "=", 
      RowBox[{
       RowBox[{"mm", "\[LeftDoubleBracket]", "i", "\[RightDoubleBracket]"}], 
       " ", 
       RowBox[{"DiagonalMatrix", "[", 
        RowBox[{"{", 
         RowBox[{"1", ",", "1", ",", "1"}], "}"}], "]"}]}]}]}], "]"}], ";"}], 
  "\n", 
  RowBox[{"(*", 
   RowBox[{
    RowBox[{"add", " ", "inertia", " ", "for", " ", "torso", " ", 
     RowBox[{"(", 
      RowBox[{"2.5", " ", "cm", " ", "radius"}], ")"}]}], ",", 
    RowBox[{"the", " ", "x"}], ",", 
    RowBox[{
    "z", " ", "terms", " ", "will", " ", "not", " ", "show", " ", "up", " ", 
     "in", " ", "the", " ", "EOM", " ", "for", " ", "this", " ", "2", "d", 
     " ", "model"}]}], "*)"}]}], "\[IndentingNewLine]", 
 RowBox[{
  RowBox[{
   SubscriptBox["\[ScriptCapitalI]", "1"], "=", 
   RowBox[{"Rationalize", "[", 
    RowBox[{
     RowBox[{
      RowBox[{"(", GridBox[{
         {"1967374.33", "0", "12.37"},
         {"0", "1946798.09", 
          RowBox[{"-", "19.96"}]},
         {"12.37", 
          RowBox[{"-", "19.96"}], "119696.29"}
        }], ")"}], "/", "1000000000"}], ",", 
     RowBox[{"1", "/", "1000000000"}]}], "]"}]}], ";"}], "\n", 
 RowBox[{
  RowBox[{
   SubscriptBox["\[ScriptCapitalI]", "2"], "=", 
   RowBox[{"Rationalize", "[", 
    RowBox[{
     RowBox[{
      RowBox[{"(", GridBox[{
         {"6494948.89", "58.80", "2024.23"},
         {"58.80", "6396011.40", "146697.91"},
         {"2024.23", "146697.91", "418371.07"}
        }], ")"}], "/", "1000000000"}], ",", 
     RowBox[{"1", "/", "1000000000"}]}], "]"}]}], ";"}], "\n", 
 RowBox[{
  RowBox[{
   SubscriptBox["\[ScriptCapitalI]", "3"], "=", 
   RowBox[{"Rationalize", "[", 
    RowBox[{
     RowBox[{
      RowBox[{"(", GridBox[{
         {"3730232.81", 
          RowBox[{"-", "7.44"}], "1948.96"},
         {
          RowBox[{"-", "7.44"}], "518271.83", 
          RowBox[{"-", "1348.28"}]},
         {"1948.96", 
          RowBox[{"-", "1348.28"}], "3577190.08"}
        }], ")"}], "/", "1000000000"}], ",", 
     RowBox[{"1", "/", "1000000000"}]}], "]"}]}], ";"}], "\n", 
 RowBox[{
  RowBox[{
   SubscriptBox["\[ScriptCapitalI]", "4"], "=", 
   RowBox[{"Rationalize", "[", 
    RowBox[{
     RowBox[{
      RowBox[{"(", GridBox[{
         {"6494948.89", 
          RowBox[{"-", "60.11"}], "1985.63"},
         {
          RowBox[{"-", "60.11"}], "6396011.38", 
          RowBox[{"-", "146697.91"}]},
         {"1985.63", 
          RowBox[{"-", "146697.91"}], "418371.06"}
        }], ")"}], "/", "1000000000"}], ",", 
     RowBox[{"1", "/", "1000000000"}]}], "]"}]}], ";"}], "\n", 
 RowBox[{
  RowBox[{
   SubscriptBox["\[ScriptCapitalI]", "5"], "=", 
   RowBox[{"Rationalize", "[", 
    RowBox[{
     RowBox[{
      RowBox[{"(", GridBox[{
         {"1967374.33", "0", 
          RowBox[{"-", "12.37"}]},
         {"0", "1946798.091", "19.96"},
         {
          RowBox[{"-", "12.37"}], "19.96", "119696.29"}
        }], ")"}], "/", "1000000000"}], ",", 
     RowBox[{"1", "/", "1000000000"}]}], "]"}]}], ";"}], "\n", 
 RowBox[{"For", "[", 
  RowBox[{
   RowBox[{"i", "=", "1"}], ",", 
   RowBox[{"i", "\[LessEqual]", "5"}], ",", 
   RowBox[{"i", "++"}], ",", 
   RowBox[{
    SubscriptBox["\[ScriptCapitalM]", "i"], "=", 
    RowBox[{"Join", "[", 
     RowBox[{
      RowBox[{"Join", "[", 
       RowBox[{
        SubscriptBox["\[ScriptCapitalM]", "i"], ",", 
        RowBox[{"Table", "[", 
         RowBox[{"0", ",", 
          RowBox[{"{", "3", "}"}], ",", 
          RowBox[{"{", "3", "}"}]}], "]"}], ",", "2"}], "]"}], ",", 
      RowBox[{"Join", "[", 
       RowBox[{
        RowBox[{"Table", "[", 
         RowBox[{"0", ",", 
          RowBox[{"{", "3", "}"}], ",", 
          RowBox[{"{", "3", "}"}]}], "]"}], ",", 
        SubscriptBox["\[ScriptCapitalI]", "i"], ",", "2"}], "]"}], ",", "1"}],
      "]"}]}]}], "]"}], "\n", 
 RowBox[{
  RowBox[{
   RowBox[{"p0", "=", 
    RowBox[{"{", 
     RowBox[{
      RowBox[{
       RowBox[{
        SubscriptBox["p", "x"], "[", "t", "]"}], "\[Rule]", "0"}], ",", 
      RowBox[{
       RowBox[{
        SubscriptBox["p", "z"], "[", "t", "]"}], "\[Rule]", "0"}], ",", 
      RowBox[{
       RowBox[{
        RowBox[{
         SubscriptBox["p", "x"], "'"}], "[", "t", "]"}], "\[Rule]", "0"}], 
      ",", 
      RowBox[{
       RowBox[{
        RowBox[{
         SubscriptBox["p", "z"], "'"}], "[", "t", "]"}], "\[Rule]", "0"}]}], 
     "}"}]}], ";"}], "\[IndentingNewLine]", 
  RowBox[{"(*", 
   RowBox[{"generalized", " ", "coordinates"}], "*)"}]}], "\n", 
 RowBox[{
  RowBox[{"q", "=", 
   RowBox[{"Table", "[", 
    RowBox[{
     RowBox[{"{", 
      RowBox[{
       SubscriptBox["\[Theta]", "i"], "[", "t", "]"}], "}"}], ",", 
     RowBox[{"{", 
      RowBox[{"i", ",", "1", ",", "ndof"}], "}"}]}], "]"}]}], 
  ";"}], "\[IndentingNewLine]", 
 RowBox[{
  RowBox[{"dq", "=", 
   RowBox[{
    SubscriptBox["\[PartialD]", "t"], "q"}]}], ";"}], "\n", 
 RowBox[{
  RowBox[{"ddq", "=", 
   RowBox[{
    SubscriptBox["\[PartialD]", "t"], "dq"}]}], ";"}], "\n", 
 RowBox[{
  RowBox[{"qe", "=", 
   RowBox[{"Join", "[", 
    RowBox[{
     RowBox[{"{", 
      RowBox[{
       RowBox[{"{", 
        RowBox[{
         SubscriptBox["p", "x"], "[", "t", "]"}], "}"}], ",", 
       RowBox[{"{", 
        RowBox[{
         SubscriptBox["p", "z"], "[", "t", "]"}], "}"}]}], "}"}], ",", "q"}], 
    "]"}]}], ";"}], "\n", 
 RowBox[{
  RowBox[{
   RowBox[{"dqe", "=", 
    RowBox[{
     SubscriptBox["\[PartialD]", "t"], "qe"}]}], ";"}], "\n", 
  RowBox[{"(*", 
   RowBox[{
   "location", " ", "and", " ", "direction", " ", "of", " ", "twists"}], 
   "*)"}]}], "\[IndentingNewLine]", 
 RowBox[{
  RowBox[{
   SubscriptBox["\[Xi]", "0"], "=", 
   RowBox[{"{", 
    RowBox[{"0", ",", "0", ",", "0", ",", "0", ",", "0", ",", "0"}], "}"}]}], 
  ";"}], "\n", 
 RowBox[{
  RowBox[{
   SubscriptBox["\[Xi]", "px"], "=", 
   RowBox[{"Simplify", "[", 
    RowBox[{
     RowBox[{"PrismaticTwist", "[", 
      RowBox[{
       RowBox[{"{", 
        RowBox[{"0", ",", "0", ",", "0"}], "}"}], ",", 
       RowBox[{"{", 
        RowBox[{"1", ",", "0", ",", "0"}], "}"}]}], "]"}], "/.", 
     "\[InvisibleSpace]", "constsubs"}], "]"}]}], ";"}], "\n", 
 RowBox[{
  RowBox[{
   SubscriptBox["\[Xi]", "pz"], "=", 
   RowBox[{"Simplify", "[", 
    RowBox[{
     RowBox[{"PrismaticTwist", "[", 
      RowBox[{
       RowBox[{"{", 
        RowBox[{"0", ",", "0", ",", "0"}], "}"}], ",", 
       RowBox[{"{", 
        RowBox[{"0", ",", "0", ",", "1"}], "}"}]}], "]"}], "/.", 
     "\[InvisibleSpace]", "constsubs"}], "]"}]}], ";"}], "\n", 
 RowBox[{
  RowBox[{
   SubscriptBox["\[Xi]", "q1"], "=", 
   RowBox[{"Simplify", "[", 
    RowBox[{
     RowBox[{"RevoluteTwist", "[", 
      RowBox[{
       RowBox[{"{", 
        RowBox[{"0", ",", "0", ",", "0"}], "}"}], ",", 
       RowBox[{"{", 
        RowBox[{"0", ",", 
         RowBox[{"-", "1"}], ",", "0"}], "}"}]}], "]"}], "/.", 
     "\[InvisibleSpace]", "constsubs"}], "]"}]}], ";"}], "\n", 
 RowBox[{
  RowBox[{
   SubscriptBox["\[Xi]", "q2"], "=", 
   RowBox[{"Simplify", "[", 
    RowBox[{
     RowBox[{"RevoluteTwist", "[", 
      RowBox[{
       RowBox[{"{", 
        RowBox[{"0", ",", "0", ",", "Lc"}], "}"}], ",", 
       RowBox[{"{", 
        RowBox[{"0", ",", 
         RowBox[{"-", "1"}], ",", "0"}], "}"}]}], "]"}], "/.", 
     "\[InvisibleSpace]", "constsubs"}], "]"}]}], ";"}], "\n", 
 RowBox[{
  RowBox[{
   SubscriptBox["\[Xi]", "q3"], "=", 
   RowBox[{"Simplify", "[", 
    RowBox[{
     RowBox[{"RevoluteTwist", "[", 
      RowBox[{
       RowBox[{"{", 
        RowBox[{"0", ",", "0", ",", 
         RowBox[{"Lc", "+", "Lt"}]}], "}"}], ",", 
       RowBox[{"{", 
        RowBox[{"0", ",", 
         RowBox[{"-", "1"}], ",", "0"}], "}"}]}], "]"}], "/.", 
     "\[InvisibleSpace]", "constsubs"}], "]"}]}], ";"}], "\n", 
 RowBox[{
  RowBox[{
   SubscriptBox["\[Xi]", "q4"], "=", 
   RowBox[{"Simplify", "[", 
    RowBox[{
     RowBox[{"RevoluteTwist", "[", 
      RowBox[{
       RowBox[{"{", 
        RowBox[{"0", ",", "0", ",", 
         RowBox[{"Lc", "+", "Lt"}]}], "}"}], ",", 
       RowBox[{"{", 
        RowBox[{"0", ",", "1", ",", "0"}], "}"}]}], "]"}], "/.", 
     "\[InvisibleSpace]", "constsubs"}], "]"}]}], ";"}], "\n", 
 RowBox[{
  RowBox[{
   RowBox[{
    SubscriptBox["\[Xi]", "q5"], "=", 
    RowBox[{"Simplify", "[", 
     RowBox[{
      RowBox[{"RevoluteTwist", "[", 
       RowBox[{
        RowBox[{"{", 
         RowBox[{"0", ",", "0", ",", "Lc"}], "}"}], ",", 
        RowBox[{"{", 
         RowBox[{"0", ",", "1", ",", "0"}], "}"}]}], "]"}], "/.", 
      "\[InvisibleSpace]", "constsubs"}], "]"}]}], ";"}], 
  "\[IndentingNewLine]", 
  RowBox[{"(*", 
   RowBox[{"base", " ", "configuration"}], "*)"}]}], "\n", 
 RowBox[{
  RowBox[{
   RowBox[{
    SubscriptBox["g", 
     SubscriptBox["sl", "1"]], "[", "0", "]"}], "=", 
   RowBox[{"Simplify", "[", 
    RowBox[{
     RowBox[{"RPToHomogeneous", "[", 
      RowBox[{
       RowBox[{"IdentityMatrix", "[", "3", "]"}], ",", 
       RowBox[{"{", 
        RowBox[{"0", ",", 
         FractionBox[
          RowBox[{"-", "33"}], "100000"], ",", "rc"}], "}"}]}], "]"}], "/.", 
     "\[InvisibleSpace]", "constsubs"}], "]"}]}], ";"}], "\n", 
 RowBox[{
  RowBox[{
   RowBox[{
    SubscriptBox["g", 
     SubscriptBox["sl", "2"]], "[", "0", "]"}], "=", 
   RowBox[{"Simplify", "[", 
    RowBox[{
     RowBox[{"RPToHomogeneous", "[", 
      RowBox[{
       RowBox[{"IdentityMatrix", "[", "3", "]"}], ",", 
       RowBox[{"{", 
        RowBox[{
         FractionBox["2", "100000"], ",", 
         FractionBox[
          RowBox[{"-", "381"}], "100000"], ",", 
         RowBox[{"Lc", "+", "rt"}]}], "}"}]}], "]"}], "/.", 
     "\[InvisibleSpace]", "constsubs"}], "]"}]}], ";"}], "\n", 
 RowBox[{
  RowBox[{
   RowBox[{
    RowBox[{
     SubscriptBox["g", 
      SubscriptBox["sl", "3"]], "[", "0", "]"}], "=", 
    RowBox[{"Simplify", "[", 
     RowBox[{
      RowBox[{"RPToHomogeneous", "[", 
       RowBox[{
        RowBox[{"IdentityMatrix", "[", "3", "]"}], ",", 
        RowBox[{"{", 
         RowBox[{
          FractionBox[
           RowBox[{"-", "24"}], "100000"], ",", 
          FractionBox[
           RowBox[{"-", "4"}], "100000"], ",", 
          RowBox[{"Lc", "+", "Lt", "+", "rT"}]}], "}"}]}], "]"}], "/.", 
      "\[InvisibleSpace]", "constsubs"}], "]"}]}], ";"}], 
  "\[IndentingNewLine]", 
  RowBox[{"(*", 
   RowBox[{
    RowBox[{
     SubscriptBox["g", 
      SubscriptBox["sl", "4"]], "[", "0", "]"}], "=", 
    RowBox[{"Simplify", "[", 
     RowBox[{
      RowBox[{"RPToHomogeneous", "[", 
       RowBox[{
        RowBox[{"IdentityMatrix", "[", "3", "]"}], ",", 
        RowBox[{"{", 
         RowBox[{
          FractionBox[
           RowBox[{"-", "24"}], "100000"], ",", 
          FractionBox[
           RowBox[{"-", "4"}], "100000"], ",", 
          RowBox[{"Lc", "+", "Lt", "+", "Lb"}]}], "}"}]}], "]"}], "/.", 
      "\[InvisibleSpace]", "constsubs"}], "]"}]}], 
   "*)"}]}], "\[IndentingNewLine]", 
 RowBox[{
  RowBox[{
   RowBox[{
    SubscriptBox["g", 
     SubscriptBox["sl", "4"]], "[", "0", "]"}], "=", 
   RowBox[{"Simplify", "[", 
    RowBox[{
     RowBox[{"RPToHomogeneous", "[", 
      RowBox[{
       RowBox[{"IdentityMatrix", "[", "3", "]"}], ",", 
       RowBox[{"{", 
        RowBox[{
         FractionBox["2", "100000"], ",", 
         FractionBox["381", "100000"], ",", 
         RowBox[{"Lc", "+", "rt"}]}], "}"}]}], "]"}], "/.", 
     "\[InvisibleSpace]", "constsubs"}], "]"}]}], ";"}], "\n", 
 RowBox[{
  RowBox[{
   RowBox[{
    SubscriptBox["g", 
     SubscriptBox["sl", "5"]], "[", "0", "]"}], "=", 
   RowBox[{"Simplify", "[", 
    RowBox[{
     RowBox[{"RPToHomogeneous", "[", 
      RowBox[{
       RowBox[{"IdentityMatrix", "[", "3", "]"}], ",", 
       RowBox[{"{", 
        RowBox[{"0", ",", 
         FractionBox["33", "100000"], ",", "rc"}], "}"}]}], "]"}], "/.", 
     "\[InvisibleSpace]", "constsubs"}], "]"}]}], ";"}]}], "Input",
 CellChangeTimes->{
  3.504875918224949*^9, {3.505063952502133*^9, 3.505063953970308*^9}, {
   3.5051489336912956`*^9, 3.5051489340032964`*^9}, 3.513531757509935*^9, {
   3.515870024222504*^9, 3.5158700252255616`*^9}, 3.5158863755610266`*^9, {
   3.5271753048660097`*^9, 3.527175316328361*^9}, {3.5271784037700644`*^9, 
   3.527178409550562*^9}, {3.527194806620797*^9, 3.527194812727977*^9}, {
   3.527194901003693*^9, 3.527194939760748*^9}, 3.527195000990027*^9, 
   3.527195065536849*^9, {3.527197843268285*^9, 3.527197851612855*^9}, {
   3.527266876626479*^9, 3.5272669091021013`*^9}, 3.5272680582479362`*^9, {
   3.52726826837751*^9, 3.5272682685913153`*^9}, {3.527268323380795*^9, 
   3.52726834320557*^9}, {3.527268721025804*^9, 3.5272687345158367`*^9}, 
   3.527271522279252*^9, {3.527280823762142*^9, 3.5272808449935427`*^9}}],

Cell[BoxData[{
 RowBox[{
  RowBox[{
   SubscriptBox["\[ScriptCapitalJ]", "1"], "=", 
   RowBox[{"Simplify", "[", 
    RowBox[{"BodyJacobian", "[", 
     RowBox[{
      RowBox[{"{", 
       RowBox[{
        SubscriptBox["\[Xi]", "px"], ",", 
        RowBox[{
         SubscriptBox["p", "x"], "[", "t", "]"}]}], "}"}], ",", 
      RowBox[{"{", 
       RowBox[{
        SubscriptBox["\[Xi]", "pz"], ",", 
        RowBox[{
         SubscriptBox["p", "z"], "[", "t", "]"}]}], "}"}], ",", 
      RowBox[{"{", 
       RowBox[{
        SubscriptBox["\[Xi]", "q1"], ",", 
        RowBox[{
         SubscriptBox["\[Theta]", "1"], "[", "t", "]"}]}], "}"}], ",", 
      RowBox[{"{", 
       RowBox[{
        SubscriptBox["\[Xi]", "0"], ",", 
        RowBox[{
         SubscriptBox["\[Theta]", "2"], "[", "t", "]"}]}], "}"}], ",", 
      RowBox[{"{", 
       RowBox[{
        SubscriptBox["\[Xi]", "0"], ",", 
        RowBox[{
         SubscriptBox["\[Theta]", "3"], "[", "t", "]"}]}], "}"}], ",", 
      RowBox[{"{", 
       RowBox[{
        SubscriptBox["\[Xi]", "0"], ",", 
        RowBox[{
         SubscriptBox["\[Theta]", "4"], "[", "t", "]"}]}], "}"}], ",", 
      RowBox[{"{", 
       RowBox[{
        SubscriptBox["\[Xi]", "0"], ",", 
        RowBox[{
         SubscriptBox["\[Theta]", "5"], "[", "t", "]"}]}], "}"}], ",", 
      RowBox[{
       SubscriptBox["g", 
        SubscriptBox["sl", "1"]], "[", "0", "]"}]}], "]"}], "]"}]}], 
  ";"}], "\n", 
 RowBox[{
  RowBox[{
   SubscriptBox["\[ScriptCapitalJ]", "2"], "=", 
   RowBox[{"Simplify", "[", 
    RowBox[{"BodyJacobian", "[", 
     RowBox[{
      RowBox[{"{", 
       RowBox[{
        SubscriptBox["\[Xi]", "px"], ",", 
        RowBox[{
         SubscriptBox["p", "x"], "[", "t", "]"}]}], "}"}], ",", 
      RowBox[{"{", 
       RowBox[{
        SubscriptBox["\[Xi]", "pz"], ",", 
        RowBox[{
         SubscriptBox["p", "z"], "[", "t", "]"}]}], "}"}], ",", 
      RowBox[{"{", 
       RowBox[{
        SubscriptBox["\[Xi]", "q1"], ",", 
        RowBox[{
         SubscriptBox["\[Theta]", "1"], "[", "t", "]"}]}], "}"}], ",", 
      RowBox[{"{", 
       RowBox[{
        SubscriptBox["\[Xi]", "q2"], ",", 
        RowBox[{
         SubscriptBox["\[Theta]", "2"], "[", "t", "]"}]}], "}"}], ",", 
      RowBox[{"{", 
       RowBox[{
        SubscriptBox["\[Xi]", "0"], ",", 
        RowBox[{
         SubscriptBox["\[Theta]", "3"], "[", "t", "]"}]}], "}"}], ",", 
      RowBox[{"{", 
       RowBox[{
        SubscriptBox["\[Xi]", "0"], ",", 
        RowBox[{
         SubscriptBox["\[Theta]", "4"], "[", "t", "]"}]}], "}"}], ",", 
      RowBox[{"{", 
       RowBox[{
        SubscriptBox["\[Xi]", "0"], ",", 
        RowBox[{
         SubscriptBox["\[Theta]", "5"], "[", "t", "]"}]}], "}"}], ",", 
      RowBox[{
       SubscriptBox["g", 
        SubscriptBox["sl", "2"]], "[", "0", "]"}]}], "]"}], "]"}]}], 
  ";"}], "\n", 
 RowBox[{
  RowBox[{
   SubscriptBox["\[ScriptCapitalJ]", "3"], "=", 
   RowBox[{"Simplify", "[", 
    RowBox[{"BodyJacobian", "[", 
     RowBox[{
      RowBox[{"{", 
       RowBox[{
        SubscriptBox["\[Xi]", "px"], ",", 
        RowBox[{
         SubscriptBox["p", "x"], "[", "t", "]"}]}], "}"}], ",", 
      RowBox[{"{", 
       RowBox[{
        SubscriptBox["\[Xi]", "pz"], ",", 
        RowBox[{
         SubscriptBox["p", "z"], "[", "t", "]"}]}], "}"}], ",", 
      RowBox[{"{", 
       RowBox[{
        SubscriptBox["\[Xi]", "q1"], ",", 
        RowBox[{
         SubscriptBox["\[Theta]", "1"], "[", "t", "]"}]}], "}"}], ",", 
      RowBox[{"{", 
       RowBox[{
        SubscriptBox["\[Xi]", "q2"], ",", 
        RowBox[{
         SubscriptBox["\[Theta]", "2"], "[", "t", "]"}]}], "}"}], ",", 
      RowBox[{"{", 
       RowBox[{
        SubscriptBox["\[Xi]", "q3"], ",", 
        RowBox[{
         SubscriptBox["\[Theta]", "3"], "[", "t", "]"}]}], "}"}], ",", 
      RowBox[{"{", 
       RowBox[{
        SubscriptBox["\[Xi]", "0"], ",", 
        RowBox[{
         SubscriptBox["\[Theta]", "4"], "[", "t", "]"}]}], "}"}], ",", 
      RowBox[{"{", 
       RowBox[{
        SubscriptBox["\[Xi]", "0"], ",", 
        RowBox[{
         SubscriptBox["\[Theta]", "5"], "[", "t", "]"}]}], "}"}], ",", 
      RowBox[{
       SubscriptBox["g", 
        SubscriptBox["sl", "3"]], "[", "0", "]"}]}], "]"}], "]"}]}], 
  ";"}], "\n", 
 RowBox[{
  RowBox[{
   SubscriptBox["\[ScriptCapitalJ]", "4"], "=", 
   RowBox[{"Simplify", "[", 
    RowBox[{"BodyJacobian", "[", 
     RowBox[{
      RowBox[{"{", 
       RowBox[{
        SubscriptBox["\[Xi]", "px"], ",", 
        RowBox[{
         SubscriptBox["p", "x"], "[", "t", "]"}]}], "}"}], ",", 
      RowBox[{"{", 
       RowBox[{
        SubscriptBox["\[Xi]", "pz"], ",", 
        RowBox[{
         SubscriptBox["p", "z"], "[", "t", "]"}]}], "}"}], ",", 
      RowBox[{"{", 
       RowBox[{
        SubscriptBox["\[Xi]", "q1"], ",", 
        RowBox[{
         SubscriptBox["\[Theta]", "1"], "[", "t", "]"}]}], "}"}], ",", 
      RowBox[{"{", 
       RowBox[{
        SubscriptBox["\[Xi]", "q2"], ",", 
        RowBox[{
         SubscriptBox["\[Theta]", "2"], "[", "t", "]"}]}], "}"}], ",", 
      RowBox[{"{", 
       RowBox[{
        SubscriptBox["\[Xi]", "q3"], ",", 
        RowBox[{
         SubscriptBox["\[Theta]", "3"], "[", "t", "]"}]}], "}"}], ",", 
      RowBox[{"{", 
       RowBox[{
        SubscriptBox["\[Xi]", "q4"], ",", 
        RowBox[{
         SubscriptBox["\[Theta]", "4"], "[", "t", "]"}]}], "}"}], ",", 
      RowBox[{"{", 
       RowBox[{
        SubscriptBox["\[Xi]", "0"], ",", 
        RowBox[{
         SubscriptBox["\[Theta]", "5"], "[", "t", "]"}]}], "}"}], ",", 
      RowBox[{
       SubscriptBox["g", 
        SubscriptBox["sl", "4"]], "[", "0", "]"}]}], "]"}], "]"}]}], 
  ";"}], "\n", 
 RowBox[{
  RowBox[{
   SubscriptBox["\[ScriptCapitalJ]", "5"], "=", 
   RowBox[{"Simplify", "[", 
    RowBox[{"BodyJacobian", "[", 
     RowBox[{
      RowBox[{"{", 
       RowBox[{
        SubscriptBox["\[Xi]", "px"], ",", 
        RowBox[{
         SubscriptBox["p", "x"], "[", "t", "]"}]}], "}"}], ",", 
      RowBox[{"{", 
       RowBox[{
        SubscriptBox["\[Xi]", "pz"], ",", 
        RowBox[{
         SubscriptBox["p", "z"], "[", "t", "]"}]}], "}"}], ",", 
      RowBox[{"{", 
       RowBox[{
        SubscriptBox["\[Xi]", "q1"], ",", 
        RowBox[{
         SubscriptBox["\[Theta]", "1"], "[", "t", "]"}]}], "}"}], ",", 
      RowBox[{"{", 
       RowBox[{
        SubscriptBox["\[Xi]", "q2"], ",", 
        RowBox[{
         SubscriptBox["\[Theta]", "2"], "[", "t", "]"}]}], "}"}], ",", 
      RowBox[{"{", 
       RowBox[{
        SubscriptBox["\[Xi]", "q3"], ",", 
        RowBox[{
         SubscriptBox["\[Theta]", "3"], "[", "t", "]"}]}], "}"}], ",", 
      RowBox[{"{", 
       RowBox[{
        SubscriptBox["\[Xi]", "q4"], ",", 
        RowBox[{
         SubscriptBox["\[Theta]", "4"], "[", "t", "]"}]}], "}"}], ",", 
      RowBox[{"{", 
       RowBox[{
        SubscriptBox["\[Xi]", "q5"], ",", 
        RowBox[{
         SubscriptBox["\[Theta]", "5"], "[", "t", "]"}]}], "}"}], ",", 
      RowBox[{
       SubscriptBox["g", 
        SubscriptBox["sl", "5"]], "[", "0", "]"}]}], "]"}], "]"}]}], 
  ";"}]}], "Input",
 CellChangeTimes->{
  3.504875918224949*^9, {3.505063952502133*^9, 3.505063953970308*^9}, {
   3.5051489336912956`*^9, 3.5051489340032964`*^9}, 3.513531757509935*^9, {
   3.515870024222504*^9, 3.5158700252255616`*^9}, 3.5158863755610266`*^9, {
   3.5271753048660097`*^9, 3.527175316328361*^9}, {3.5271784037700644`*^9, 
   3.527178409550562*^9}, {3.527194806620797*^9, 3.527194812727977*^9}, {
   3.527194901003693*^9, 3.527194939760748*^9}, 3.527195000990027*^9, 
   3.527195065536849*^9, {3.527197843268285*^9, 3.527197851612855*^9}, {
   3.527266876626479*^9, 3.5272669091021013`*^9}, 3.5272680582479362`*^9, {
   3.52726826837751*^9, 3.5272682685913153`*^9}, {3.527268323380795*^9, 
   3.52726834320557*^9}, {3.527268721025804*^9, 3.5272687345158367`*^9}, 
   3.527271522279252*^9, {3.527280823762142*^9, 3.527280828414195*^9}, 
   3.5272808762225933`*^9, {3.527281589348708*^9, 3.52728158963411*^9}, {
   3.5272816841443167`*^9, 3.5272817411589403`*^9}}],

Cell[CellGroupData[{

Cell[BoxData[
 RowBox[{
  RowBox[{"(*", 
   RowBox[{
   "calculate", " ", "the", " ", "forward", " ", "kinematics", " ", "maps"}], 
   "*)"}], "\[IndentingNewLine]", 
  RowBox[{
   RowBox[{
    RowBox[{
     SubscriptBox["g", "1"], "[", "\[Theta]", "]"}], "=", 
    RowBox[{"Simplify", "[", 
     RowBox[{
      RowBox[{"ForwardKinematics", "[", 
       RowBox[{
        RowBox[{"{", 
         RowBox[{
          SubscriptBox["\[Xi]", "px"], ",", 
          RowBox[{
           SubscriptBox["p", "x"], "[", "t", "]"}]}], "}"}], ",", 
        RowBox[{"{", 
         RowBox[{
          SubscriptBox["\[Xi]", "pz"], ",", 
          RowBox[{
           SubscriptBox["p", "z"], "[", "t", "]"}]}], "}"}], ",", 
        RowBox[{"{", 
         RowBox[{
          SubscriptBox["\[Xi]", "q1"], ",", 
          RowBox[{
           SubscriptBox["\[Theta]", "1"], "[", "t", "]"}]}], "}"}], ",", 
        RowBox[{
         SubscriptBox["g", 
          SubscriptBox["sl", "1"]], "[", "0", "]"}]}], "]"}], "/.", 
      "constsubs"}], "]"}]}], "\n", 
   RowBox[{
    RowBox[{
     SubscriptBox["g", "2"], "[", "\[Theta]", "]"}], "=", 
    RowBox[{"Simplify", "[", 
     RowBox[{
      RowBox[{"ForwardKinematics", "[", 
       RowBox[{
        RowBox[{"{", 
         RowBox[{
          SubscriptBox["\[Xi]", "px"], ",", 
          RowBox[{
           SubscriptBox["p", "x"], "[", "t", "]"}]}], "}"}], ",", 
        RowBox[{"{", 
         RowBox[{
          SubscriptBox["\[Xi]", "pz"], ",", 
          RowBox[{
           SubscriptBox["p", "z"], "[", "t", "]"}]}], "}"}], ",", 
        RowBox[{"{", 
         RowBox[{
          SubscriptBox["\[Xi]", "q1"], ",", 
          RowBox[{
           SubscriptBox["\[Theta]", "1"], "[", "t", "]"}]}], "}"}], ",", 
        RowBox[{"{", 
         RowBox[{
          SubscriptBox["\[Xi]", "q2"], ",", 
          RowBox[{
           SubscriptBox["\[Theta]", "2"], "[", "t", "]"}]}], "}"}], ",", 
        RowBox[{
         SubscriptBox["g", 
          SubscriptBox["sl", "2"]], "[", "0", "]"}]}], "]"}], "/.", 
      "constsubs"}], "]"}]}], "\n", 
   RowBox[{
    RowBox[{
     SubscriptBox["g", "3"], "[", "\[Theta]", "]"}], "=", 
    RowBox[{"Simplify", "[", 
     RowBox[{
      RowBox[{"ForwardKinematics", "[", 
       RowBox[{
        RowBox[{"{", 
         RowBox[{
          SubscriptBox["\[Xi]", "px"], ",", 
          RowBox[{
           SubscriptBox["p", "x"], "[", "t", "]"}]}], "}"}], ",", 
        RowBox[{"{", 
         RowBox[{
          SubscriptBox["\[Xi]", "pz"], ",", 
          RowBox[{
           SubscriptBox["p", "z"], "[", "t", "]"}]}], "}"}], ",", 
        RowBox[{"{", 
         RowBox[{
          SubscriptBox["\[Xi]", "q1"], ",", 
          RowBox[{
           SubscriptBox["\[Theta]", "1"], "[", "t", "]"}]}], "}"}], ",", 
        RowBox[{"{", 
         RowBox[{
          SubscriptBox["\[Xi]", "q2"], ",", 
          RowBox[{
           SubscriptBox["\[Theta]", "2"], "[", "t", "]"}]}], "}"}], ",", 
        RowBox[{"{", 
         RowBox[{
          SubscriptBox["\[Xi]", "q3"], ",", 
          RowBox[{
           SubscriptBox["\[Theta]", "3"], "[", "t", "]"}]}], "}"}], ",", 
        RowBox[{
         SubscriptBox["g", 
          SubscriptBox["sl", "3"]], "[", "0", "]"}]}], "]"}], "/.", 
      "constsubs"}], "]"}]}], "\n", 
   RowBox[{
    RowBox[{
     SubscriptBox["g", "4"], "[", "\[Theta]", "]"}], "=", 
    RowBox[{"Simplify", "[", 
     RowBox[{
      RowBox[{"ForwardKinematics", "[", 
       RowBox[{
        RowBox[{"{", 
         RowBox[{
          SubscriptBox["\[Xi]", "px"], ",", 
          RowBox[{
           SubscriptBox["p", "x"], "[", "t", "]"}]}], "}"}], ",", 
        RowBox[{"{", 
         RowBox[{
          SubscriptBox["\[Xi]", "pz"], ",", 
          RowBox[{
           SubscriptBox["p", "z"], "[", "t", "]"}]}], "}"}], ",", 
        RowBox[{"{", 
         RowBox[{
          SubscriptBox["\[Xi]", "q1"], ",", 
          RowBox[{
           SubscriptBox["\[Theta]", "1"], "[", "t", "]"}]}], "}"}], ",", 
        RowBox[{"{", 
         RowBox[{
          SubscriptBox["\[Xi]", "q2"], ",", 
          RowBox[{
           SubscriptBox["\[Theta]", "2"], "[", "t", "]"}]}], "}"}], ",", 
        RowBox[{"{", 
         RowBox[{
          SubscriptBox["\[Xi]", "q3"], ",", 
          RowBox[{
           SubscriptBox["\[Theta]", "3"], "[", "t", "]"}]}], "}"}], ",", 
        RowBox[{"{", 
         RowBox[{
          SubscriptBox["\[Xi]", "q4"], ",", 
          RowBox[{
           SubscriptBox["\[Theta]", "4"], "[", "t", "]"}]}], "}"}], ",", 
        RowBox[{
         SubscriptBox["g", 
          SubscriptBox["sl", "4"]], "[", "0", "]"}]}], "]"}], "/.", 
      "constsubs"}], "]"}]}], "\n", 
   RowBox[{
    RowBox[{
     RowBox[{
      SubscriptBox["g", "5"], "[", "\[Theta]", "]"}], "=", 
     RowBox[{"Simplify", "[", 
      RowBox[{
       RowBox[{"ForwardKinematics", "[", 
        RowBox[{
         RowBox[{"{", 
          RowBox[{
           SubscriptBox["\[Xi]", "px"], ",", 
           RowBox[{
            SubscriptBox["p", "x"], "[", "t", "]"}]}], "}"}], ",", 
         RowBox[{"{", 
          RowBox[{
           SubscriptBox["\[Xi]", "pz"], ",", 
           RowBox[{
            SubscriptBox["p", "z"], "[", "t", "]"}]}], "}"}], ",", 
         RowBox[{"{", 
          RowBox[{
           SubscriptBox["\[Xi]", "q1"], ",", 
           RowBox[{
            SubscriptBox["\[Theta]", "1"], "[", "t", "]"}]}], "}"}], ",", 
         RowBox[{"{", 
          RowBox[{
           SubscriptBox["\[Xi]", "q2"], ",", 
           RowBox[{
            SubscriptBox["\[Theta]", "2"], "[", "t", "]"}]}], "}"}], ",", 
         RowBox[{"{", 
          RowBox[{
           SubscriptBox["\[Xi]", "q3"], ",", 
           RowBox[{
            SubscriptBox["\[Theta]", "3"], "[", "t", "]"}]}], "}"}], ",", 
         RowBox[{"{", 
          RowBox[{
           SubscriptBox["\[Xi]", "q4"], ",", 
           RowBox[{
            SubscriptBox["\[Theta]", "4"], "[", "t", "]"}]}], "}"}], ",", 
         RowBox[{"{", 
          RowBox[{
           SubscriptBox["\[Xi]", "q5"], ",", 
           RowBox[{
            SubscriptBox["\[Theta]", "5"], "[", "t", "]"}]}], "}"}], ",", 
         RowBox[{
          SubscriptBox["g", 
           SubscriptBox["sl", "5"]], "[", "0", "]"}]}], "]"}], "/.", 
       "constsubs"}], "]"}]}], ";"}]}]}]], "Input",
 CellChangeTimes->{
  3.504875918224949*^9, {3.505063952502133*^9, 3.505063953970308*^9}, {
   3.5051489336912956`*^9, 3.5051489340032964`*^9}, 3.513531757509935*^9, {
   3.515870024222504*^9, 3.5158700252255616`*^9}, 3.5158863755610266`*^9, {
   3.5271753048660097`*^9, 3.527175316328361*^9}, {3.5271784037700644`*^9, 
   3.527178409550562*^9}, {3.527194806620797*^9, 3.527194812727977*^9}, {
   3.527194901003693*^9, 3.527194939760748*^9}, 3.527195000990027*^9, 
   3.527195065536849*^9, {3.527197843268285*^9, 3.527197851612855*^9}, {
   3.527266876626479*^9, 3.5272669091021013`*^9}, 3.5272680582479362`*^9, {
   3.52726826837751*^9, 3.5272682685913153`*^9}, {3.527268323380795*^9, 
   3.52726834320557*^9}, {3.527268721025804*^9, 3.5272687345158367`*^9}, 
   3.527271522279252*^9, {3.527280823762142*^9, 3.527280828414195*^9}, 
   3.5272808762225933`*^9, {3.527281589348708*^9, 3.52728158963411*^9}, {
   3.5272816841443167`*^9, 3.527281777527067*^9}}],

Cell[BoxData[
 FormBox[
  RowBox[{"(", "\[NoBreak]", GridBox[{
     {
      RowBox[{"cos", "(", 
       RowBox[{
        SubscriptBox["\[Theta]", "1"], "(", "t", ")"}], ")"}], "0", 
      RowBox[{"-", 
       RowBox[{"sin", "(", 
        RowBox[{
         SubscriptBox["\[Theta]", "1"], "(", "t", ")"}], ")"}]}], 
      RowBox[{
       RowBox[{
        SubscriptBox["p", "x"], "(", "t", ")"}], "-", 
       FractionBox[
        RowBox[{"28237", " ", 
         RowBox[{"sin", "(", 
          RowBox[{
           SubscriptBox["\[Theta]", "1"], "(", "t", ")"}], ")"}]}], 
        "100000"]}]},
     {"0", "1", "0", 
      RowBox[{"-", 
       FractionBox["33", "100000"]}]},
     {
      RowBox[{"sin", "(", 
       RowBox[{
        SubscriptBox["\[Theta]", "1"], "(", "t", ")"}], ")"}], "0", 
      RowBox[{"cos", "(", 
       RowBox[{
        SubscriptBox["\[Theta]", "1"], "(", "t", ")"}], ")"}], 
      RowBox[{
       FractionBox[
        RowBox[{"28237", " ", 
         RowBox[{"cos", "(", 
          RowBox[{
           SubscriptBox["\[Theta]", "1"], "(", "t", ")"}], ")"}]}], "100000"],
        "+", 
       RowBox[{
        SubscriptBox["p", "z"], "(", "t", ")"}]}]},
     {"0", "0", "0", "1"}
    },
    GridBoxAlignment->{
     "Columns" -> {{Center}}, "ColumnsIndexed" -> {}, "Rows" -> {{Baseline}}, 
      "RowsIndexed" -> {}},
    GridBoxSpacings->{"Columns" -> {
        Offset[0.27999999999999997`], {
         Offset[0.7]}, 
        Offset[0.27999999999999997`]}, "ColumnsIndexed" -> {}, "Rows" -> {
        Offset[0.2], {
         Offset[0.4]}, 
        Offset[0.2]}, "RowsIndexed" -> {}}], "\[NoBreak]", ")"}], 
  TraditionalForm]], "Output",
 CellChangeTimes->{{3.527281743697872*^9, 3.527281777871376*^9}, 
   3.527281934628038*^9}],

Cell[BoxData[
 FormBox[
  RowBox[{"(", "\[NoBreak]", GridBox[{
     {
      RowBox[{"cos", "(", 
       RowBox[{
        RowBox[{
         SubscriptBox["\[Theta]", "1"], "(", "t", ")"}], "+", 
        RowBox[{
         SubscriptBox["\[Theta]", "2"], "(", "t", ")"}]}], ")"}], "0", 
      RowBox[{"-", 
       RowBox[{"sin", "(", 
        RowBox[{
         RowBox[{
          SubscriptBox["\[Theta]", "1"], "(", "t", ")"}], "+", 
         RowBox[{
          SubscriptBox["\[Theta]", "2"], "(", "t", ")"}]}], ")"}]}], 
      FractionBox[
       RowBox[{
        RowBox[{"2", " ", 
         RowBox[{"cos", "(", 
          RowBox[{
           RowBox[{
            SubscriptBox["\[Theta]", "1"], "(", "t", ")"}], "+", 
           RowBox[{
            SubscriptBox["\[Theta]", "2"], "(", "t", ")"}]}], ")"}]}], "-", 
        RowBox[{"34798", " ", 
         RowBox[{"sin", "(", 
          RowBox[{
           SubscriptBox["\[Theta]", "1"], "(", "t", ")"}], ")"}]}], "-", 
        RowBox[{"12815", " ", 
         RowBox[{"sin", "(", 
          RowBox[{
           RowBox[{
            SubscriptBox["\[Theta]", "1"], "(", "t", ")"}], "+", 
           RowBox[{
            SubscriptBox["\[Theta]", "2"], "(", "t", ")"}]}], ")"}]}], "+", 
        RowBox[{"100000", " ", 
         RowBox[{
          SubscriptBox["p", "x"], "(", "t", ")"}]}]}], "100000"]},
     {"0", "1", "0", 
      RowBox[{"-", 
       FractionBox["381", "100000"]}]},
     {
      RowBox[{"sin", "(", 
       RowBox[{
        RowBox[{
         SubscriptBox["\[Theta]", "1"], "(", "t", ")"}], "+", 
        RowBox[{
         SubscriptBox["\[Theta]", "2"], "(", "t", ")"}]}], ")"}], "0", 
      RowBox[{"cos", "(", 
       RowBox[{
        RowBox[{
         SubscriptBox["\[Theta]", "1"], "(", "t", ")"}], "+", 
        RowBox[{
         SubscriptBox["\[Theta]", "2"], "(", "t", ")"}]}], ")"}], 
      FractionBox[
       RowBox[{
        RowBox[{"34798", " ", 
         RowBox[{"cos", "(", 
          RowBox[{
           SubscriptBox["\[Theta]", "1"], "(", "t", ")"}], ")"}]}], "+", 
        RowBox[{"12815", " ", 
         RowBox[{"cos", "(", 
          RowBox[{
           RowBox[{
            SubscriptBox["\[Theta]", "1"], "(", "t", ")"}], "+", 
           RowBox[{
            SubscriptBox["\[Theta]", "2"], "(", "t", ")"}]}], ")"}]}], "+", 
        RowBox[{"2", " ", 
         RowBox[{"sin", "(", 
          RowBox[{
           RowBox[{
            SubscriptBox["\[Theta]", "1"], "(", "t", ")"}], "+", 
           RowBox[{
            SubscriptBox["\[Theta]", "2"], "(", "t", ")"}]}], ")"}]}], "+", 
        RowBox[{"100000", " ", 
         RowBox[{
          SubscriptBox["p", "z"], "(", "t", ")"}]}]}], "100000"]},
     {"0", "0", "0", "1"}
    },
    GridBoxAlignment->{
     "Columns" -> {{Center}}, "ColumnsIndexed" -> {}, "Rows" -> {{Baseline}}, 
      "RowsIndexed" -> {}},
    GridBoxSpacings->{"Columns" -> {
        Offset[0.27999999999999997`], {
         Offset[0.7]}, 
        Offset[0.27999999999999997`]}, "ColumnsIndexed" -> {}, "Rows" -> {
        Offset[0.2], {
         Offset[0.4]}, 
        Offset[0.2]}, "RowsIndexed" -> {}}], "\[NoBreak]", ")"}], 
  TraditionalForm]], "Output",
 CellChangeTimes->{{3.527281743697872*^9, 3.527281777871376*^9}, 
   3.5272819347*^9}],

Cell[BoxData[
 FormBox[
  RowBox[{"(", "\[NoBreak]", GridBox[{
     {
      RowBox[{"cos", "(", 
       RowBox[{
        RowBox[{
         SubscriptBox["\[Theta]", "1"], "(", "t", ")"}], "+", 
        RowBox[{
         SubscriptBox["\[Theta]", "2"], "(", "t", ")"}], "+", 
        RowBox[{
         SubscriptBox["\[Theta]", "3"], "(", "t", ")"}]}], ")"}], "0", 
      RowBox[{"-", 
       RowBox[{"sin", "(", 
        RowBox[{
         RowBox[{
          SubscriptBox["\[Theta]", "1"], "(", "t", ")"}], "+", 
         RowBox[{
          SubscriptBox["\[Theta]", "2"], "(", "t", ")"}], "+", 
         RowBox[{
          SubscriptBox["\[Theta]", "3"], "(", "t", ")"}]}], ")"}]}], 
      FractionBox[
       RowBox[{
        RowBox[{
         RowBox[{"-", "120"}], " ", 
         RowBox[{"cos", "(", 
          RowBox[{
           RowBox[{
            SubscriptBox["\[Theta]", "1"], "(", "t", ")"}], "+", 
           RowBox[{
            SubscriptBox["\[Theta]", "2"], "(", "t", ")"}], "+", 
           RowBox[{
            SubscriptBox["\[Theta]", "3"], "(", "t", ")"}]}], ")"}]}], "-", 
        RowBox[{"173990", " ", 
         RowBox[{"sin", "(", 
          RowBox[{
           SubscriptBox["\[Theta]", "1"], "(", "t", ")"}], ")"}]}], "-", 
        RowBox[{"130556", " ", 
         RowBox[{"sin", "(", 
          RowBox[{
           RowBox[{
            SubscriptBox["\[Theta]", "1"], "(", "t", ")"}], "+", 
           RowBox[{
            SubscriptBox["\[Theta]", "2"], "(", "t", ")"}]}], ")"}]}], "-", 
        RowBox[{"4985", " ", 
         RowBox[{"sin", "(", 
          RowBox[{
           RowBox[{
            SubscriptBox["\[Theta]", "1"], "(", "t", ")"}], "+", 
           RowBox[{
            SubscriptBox["\[Theta]", "2"], "(", "t", ")"}], "+", 
           RowBox[{
            SubscriptBox["\[Theta]", "3"], "(", "t", ")"}]}], ")"}]}], "+", 
        RowBox[{"500000", " ", 
         RowBox[{
          SubscriptBox["p", "x"], "(", "t", ")"}]}]}], "500000"]},
     {"0", "1", "0", 
      RowBox[{"-", 
       FractionBox["1", "25000"]}]},
     {
      RowBox[{"sin", "(", 
       RowBox[{
        RowBox[{
         SubscriptBox["\[Theta]", "1"], "(", "t", ")"}], "+", 
        RowBox[{
         SubscriptBox["\[Theta]", "2"], "(", "t", ")"}], "+", 
        RowBox[{
         SubscriptBox["\[Theta]", "3"], "(", "t", ")"}]}], ")"}], "0", 
      RowBox[{"cos", "(", 
       RowBox[{
        RowBox[{
         SubscriptBox["\[Theta]", "1"], "(", "t", ")"}], "+", 
        RowBox[{
         SubscriptBox["\[Theta]", "2"], "(", "t", ")"}], "+", 
        RowBox[{
         SubscriptBox["\[Theta]", "3"], "(", "t", ")"}]}], ")"}], 
      FractionBox[
       RowBox[{
        RowBox[{"173990", " ", 
         RowBox[{"cos", "(", 
          RowBox[{
           SubscriptBox["\[Theta]", "1"], "(", "t", ")"}], ")"}]}], "+", 
        RowBox[{"130556", " ", 
         RowBox[{"cos", "(", 
          RowBox[{
           RowBox[{
            SubscriptBox["\[Theta]", "1"], "(", "t", ")"}], "+", 
           RowBox[{
            SubscriptBox["\[Theta]", "2"], "(", "t", ")"}]}], ")"}]}], "+", 
        RowBox[{"4985", " ", 
         RowBox[{"cos", "(", 
          RowBox[{
           RowBox[{
            SubscriptBox["\[Theta]", "1"], "(", "t", ")"}], "+", 
           RowBox[{
            SubscriptBox["\[Theta]", "2"], "(", "t", ")"}], "+", 
           RowBox[{
            SubscriptBox["\[Theta]", "3"], "(", "t", ")"}]}], ")"}]}], "-", 
        RowBox[{"120", " ", 
         RowBox[{"sin", "(", 
          RowBox[{
           RowBox[{
            SubscriptBox["\[Theta]", "1"], "(", "t", ")"}], "+", 
           RowBox[{
            SubscriptBox["\[Theta]", "2"], "(", "t", ")"}], "+", 
           RowBox[{
            SubscriptBox["\[Theta]", "3"], "(", "t", ")"}]}], ")"}]}], "+", 
        RowBox[{"500000", " ", 
         RowBox[{
          SubscriptBox["p", "z"], "(", "t", ")"}]}]}], "500000"]},
     {"0", "0", "0", "1"}
    },
    GridBoxAlignment->{
     "Columns" -> {{Center}}, "ColumnsIndexed" -> {}, "Rows" -> {{Baseline}}, 
      "RowsIndexed" -> {}},
    GridBoxSpacings->{"Columns" -> {
        Offset[0.27999999999999997`], {
         Offset[0.7]}, 
        Offset[0.27999999999999997`]}, "ColumnsIndexed" -> {}, "Rows" -> {
        Offset[0.2], {
         Offset[0.4]}, 
        Offset[0.2]}, "RowsIndexed" -> {}}], "\[NoBreak]", ")"}], 
  TraditionalForm]], "Output",
 CellChangeTimes->{{3.527281743697872*^9, 3.527281777871376*^9}, 
   3.52728193481386*^9}],

Cell[BoxData[
 FormBox[
  RowBox[{"(", "\[NoBreak]", GridBox[{
     {
      RowBox[{"cos", "(", 
       RowBox[{
        RowBox[{
         SubscriptBox["\[Theta]", "1"], "(", "t", ")"}], "+", 
        RowBox[{
         SubscriptBox["\[Theta]", "2"], "(", "t", ")"}], "+", 
        RowBox[{
         SubscriptBox["\[Theta]", "3"], "(", "t", ")"}], "-", 
        RowBox[{
         SubscriptBox["\[Theta]", "4"], "(", "t", ")"}]}], ")"}], "0", 
      RowBox[{"-", 
       RowBox[{"sin", "(", 
        RowBox[{
         RowBox[{
          SubscriptBox["\[Theta]", "1"], "(", "t", ")"}], "+", 
         RowBox[{
          SubscriptBox["\[Theta]", "2"], "(", "t", ")"}], "+", 
         RowBox[{
          SubscriptBox["\[Theta]", "3"], "(", "t", ")"}], "-", 
         RowBox[{
          SubscriptBox["\[Theta]", "4"], "(", "t", ")"}]}], ")"}]}], 
      FractionBox[
       RowBox[{
        RowBox[{"10", " ", 
         RowBox[{"cos", "(", 
          RowBox[{
           RowBox[{
            SubscriptBox["\[Theta]", "1"], "(", "t", ")"}], "+", 
           RowBox[{
            SubscriptBox["\[Theta]", "2"], "(", "t", ")"}], "+", 
           RowBox[{
            SubscriptBox["\[Theta]", "3"], "(", "t", ")"}], "-", 
           RowBox[{
            SubscriptBox["\[Theta]", "4"], "(", "t", ")"}]}], ")"}]}], "-", 
        RowBox[{"173990", " ", 
         RowBox[{"sin", "(", 
          RowBox[{
           SubscriptBox["\[Theta]", "1"], "(", "t", ")"}], ")"}]}], "-", 
        RowBox[{"130556", " ", 
         RowBox[{"sin", "(", 
          RowBox[{
           RowBox[{
            SubscriptBox["\[Theta]", "1"], "(", "t", ")"}], "+", 
           RowBox[{
            SubscriptBox["\[Theta]", "2"], "(", "t", ")"}]}], ")"}]}], "+", 
        RowBox[{"66481", " ", 
         RowBox[{"sin", "(", 
          RowBox[{
           RowBox[{
            SubscriptBox["\[Theta]", "1"], "(", "t", ")"}], "+", 
           RowBox[{
            SubscriptBox["\[Theta]", "2"], "(", "t", ")"}], "+", 
           RowBox[{
            SubscriptBox["\[Theta]", "3"], "(", "t", ")"}], "-", 
           RowBox[{
            SubscriptBox["\[Theta]", "4"], "(", "t", ")"}]}], ")"}]}], "+", 
        RowBox[{"500000", " ", 
         RowBox[{
          SubscriptBox["p", "x"], "(", "t", ")"}]}]}], "500000"]},
     {"0", "1", "0", 
      FractionBox["381", "100000"]},
     {
      RowBox[{"sin", "(", 
       RowBox[{
        RowBox[{
         SubscriptBox["\[Theta]", "1"], "(", "t", ")"}], "+", 
        RowBox[{
         SubscriptBox["\[Theta]", "2"], "(", "t", ")"}], "+", 
        RowBox[{
         SubscriptBox["\[Theta]", "3"], "(", "t", ")"}], "-", 
        RowBox[{
         SubscriptBox["\[Theta]", "4"], "(", "t", ")"}]}], ")"}], "0", 
      RowBox[{"cos", "(", 
       RowBox[{
        RowBox[{
         SubscriptBox["\[Theta]", "1"], "(", "t", ")"}], "+", 
        RowBox[{
         SubscriptBox["\[Theta]", "2"], "(", "t", ")"}], "+", 
        RowBox[{
         SubscriptBox["\[Theta]", "3"], "(", "t", ")"}], "-", 
        RowBox[{
         SubscriptBox["\[Theta]", "4"], "(", "t", ")"}]}], ")"}], 
      FractionBox[
       RowBox[{
        RowBox[{"173990", " ", 
         RowBox[{"cos", "(", 
          RowBox[{
           SubscriptBox["\[Theta]", "1"], "(", "t", ")"}], ")"}]}], "+", 
        RowBox[{"130556", " ", 
         RowBox[{"cos", "(", 
          RowBox[{
           RowBox[{
            SubscriptBox["\[Theta]", "1"], "(", "t", ")"}], "+", 
           RowBox[{
            SubscriptBox["\[Theta]", "2"], "(", "t", ")"}]}], ")"}]}], "-", 
        RowBox[{"66481", " ", 
         RowBox[{"cos", "(", 
          RowBox[{
           RowBox[{
            SubscriptBox["\[Theta]", "1"], "(", "t", ")"}], "+", 
           RowBox[{
            SubscriptBox["\[Theta]", "2"], "(", "t", ")"}], "+", 
           RowBox[{
            SubscriptBox["\[Theta]", "3"], "(", "t", ")"}], "-", 
           RowBox[{
            SubscriptBox["\[Theta]", "4"], "(", "t", ")"}]}], ")"}]}], "+", 
        RowBox[{"10", " ", 
         RowBox[{"sin", "(", 
          RowBox[{
           RowBox[{
            SubscriptBox["\[Theta]", "1"], "(", "t", ")"}], "+", 
           RowBox[{
            SubscriptBox["\[Theta]", "2"], "(", "t", ")"}], "+", 
           RowBox[{
            SubscriptBox["\[Theta]", "3"], "(", "t", ")"}], "-", 
           RowBox[{
            SubscriptBox["\[Theta]", "4"], "(", "t", ")"}]}], ")"}]}], "+", 
        RowBox[{"500000", " ", 
         RowBox[{
          SubscriptBox["p", "z"], "(", "t", ")"}]}]}], "500000"]},
     {"0", "0", "0", "1"}
    },
    GridBoxAlignment->{
     "Columns" -> {{Center}}, "ColumnsIndexed" -> {}, "Rows" -> {{Baseline}}, 
      "RowsIndexed" -> {}},
    GridBoxSpacings->{"Columns" -> {
        Offset[0.27999999999999997`], {
         Offset[0.7]}, 
        Offset[0.27999999999999997`]}, "ColumnsIndexed" -> {}, "Rows" -> {
        Offset[0.2], {
         Offset[0.4]}, 
        Offset[0.2]}, "RowsIndexed" -> {}}], "\[NoBreak]", ")"}], 
  TraditionalForm]], "Output",
 CellChangeTimes->{{3.527281743697872*^9, 3.527281777871376*^9}, 
   3.527281935024892*^9}]
}, Open  ]],

Cell[BoxData[{
 RowBox[{
  RowBox[{
   RowBox[{
    SubscriptBox["g", "stf"], "[", "0", "]"}], "=", 
   RowBox[{
    RowBox[{"RPToHomogeneous", "[", 
     RowBox[{
      RowBox[{"IdentityMatrix", "[", "3", "]"}], ",", 
      RowBox[{"{", 
       RowBox[{"0", ",", "0", ",", "0"}], "}"}]}], "]"}], "/.", 
    "constsubs"}]}], ";"}], "\n", 
 RowBox[{
  RowBox[{
   RowBox[{
    SubscriptBox["g", "stk"], "[", "0", "]"}], "=", 
   RowBox[{
    RowBox[{"RPToHomogeneous", "[", 
     RowBox[{
      RowBox[{"IdentityMatrix", "[", "3", "]"}], ",", 
      RowBox[{"{", 
       RowBox[{"0", ",", "0", ",", "Lc"}], "}"}]}], "]"}], "/.", 
    "constsubs"}]}], ";"}], "\n", 
 RowBox[{
  RowBox[{
   RowBox[{
    SubscriptBox["g", "hip"], "[", "0", "]"}], "=", 
   RowBox[{
    RowBox[{"RPToHomogeneous", "[", 
     RowBox[{
      RowBox[{"IdentityMatrix", "[", "3", "]"}], ",", 
      RowBox[{"{", 
       RowBox[{"0", ",", "0", ",", 
        RowBox[{"Lc", "+", "Lt"}]}], "}"}]}], "]"}], "/.", "constsubs"}]}], 
  ";"}], "\[IndentingNewLine]", 
 RowBox[{
  RowBox[{
   RowBox[{
    SubscriptBox["g", "torso"], "[", "0", "]"}], "=", 
   RowBox[{
    RowBox[{"RPToHomogeneous", "[", 
     RowBox[{
      RowBox[{"IdentityMatrix", "[", "3", "]"}], ",", 
      RowBox[{"{", 
       RowBox[{"0", ",", "0", ",", 
        RowBox[{"Lc", "+", "Lt", "+", "rT"}]}], "}"}]}], "]"}], "/.", 
    "constsubs"}]}], ";"}], "\n", 
 RowBox[{
  RowBox[{
   RowBox[{
    SubscriptBox["g", "nsk"], "[", "0", "]"}], "=", 
   RowBox[{
    RowBox[{"RPToHomogeneous", "[", 
     RowBox[{
      RowBox[{"IdentityMatrix", "[", "3", "]"}], ",", 
      RowBox[{"{", 
       RowBox[{"0", ",", "0", ",", "Lc"}], "}"}]}], "]"}], "/.", 
    "constsubs"}]}], ";", 
  RowBox[{
   RowBox[{
    SubscriptBox["g", "nsf"], "[", "0", "]"}], "=", 
   RowBox[{
    RowBox[{"RPToHomogeneous", "[", 
     RowBox[{
      RowBox[{"IdentityMatrix", "[", "3", "]"}], ",", 
      RowBox[{"{", 
       RowBox[{"0", ",", "0", ",", "0"}], "}"}]}], "]"}], "/.", 
    "constsubs"}]}], ";"}]}], "Input",
 CellChangeTimes->{
  3.504875918224949*^9, {3.505063952502133*^9, 3.505063953970308*^9}, {
   3.5051489336912956`*^9, 3.5051489340032964`*^9}, 3.513531757509935*^9, {
   3.515870024222504*^9, 3.5158700252255616`*^9}, 3.5158863755610266`*^9, {
   3.5271753048660097`*^9, 3.527175316328361*^9}, {3.5271784037700644`*^9, 
   3.527178409550562*^9}, {3.527194806620797*^9, 3.527194812727977*^9}, {
   3.527194901003693*^9, 3.527194939760748*^9}, 3.527195000990027*^9, 
   3.527195065536849*^9, {3.527197843268285*^9, 3.527197851612855*^9}, {
   3.527266876626479*^9, 3.5272669091021013`*^9}, 3.5272680582479362`*^9, {
   3.52726826837751*^9, 3.5272682685913153`*^9}, {3.527268323380795*^9, 
   3.52726834320557*^9}, {3.527268721025804*^9, 3.5272687345158367`*^9}, 
   3.527271522279252*^9, {3.527280823762142*^9, 3.527280828414195*^9}, 
   3.5272808762225933`*^9, {3.527281589348708*^9, 3.52728158963411*^9}, {
   3.5272816841443167`*^9, 3.5272817333067636`*^9}}],

Cell[CellGroupData[{

Cell[BoxData[{
 RowBox[{
  RowBox[{
   RowBox[{
    SubscriptBox["g", "stf"], "[", "\[Theta]", "]"}], "=", 
   RowBox[{"Simplify", "[", 
    RowBox[{
     RowBox[{"ForwardKinematics", "[", 
      RowBox[{
       RowBox[{"{", 
        RowBox[{
         SubscriptBox["\[Xi]", "0"], ",", "0"}], "}"}], ",", 
       RowBox[{
        SubscriptBox["g", "stf"], "[", "0", "]"}]}], "]"}], "/.", 
     "constsubs"}], "]"}]}], ";"}], "\n", 
 RowBox[{
  RowBox[{
   RowBox[{
    SubscriptBox["g", "stk"], "[", "\[Theta]", "]"}], "=", 
   RowBox[{"Simplify", "[", 
    RowBox[{
     RowBox[{"ForwardKinematics", "[", 
      RowBox[{
       RowBox[{"{", 
        RowBox[{
         SubscriptBox["\[Xi]", "q1"], ",", 
         RowBox[{
          SubscriptBox["\[Theta]", "1"], "[", "t", "]"}]}], "}"}], ",", 
       RowBox[{
        SubscriptBox["g", "stk"], "[", "0", "]"}]}], "]"}], "/.", 
     "constsubs"}], "]"}]}], ";"}], "\n", 
 RowBox[{
  RowBox[{
   RowBox[{
    SubscriptBox["g", "hip"], "[", "\[Theta]", "]"}], "=", 
   RowBox[{"Simplify", "[", 
    RowBox[{
     RowBox[{"ForwardKinematics", "[", 
      RowBox[{
       RowBox[{"{", 
        RowBox[{
         SubscriptBox["\[Xi]", "q1"], ",", 
         RowBox[{
          SubscriptBox["\[Theta]", "1"], "[", "t", "]"}]}], "}"}], ",", 
       RowBox[{"{", 
        RowBox[{
         SubscriptBox["\[Xi]", "q2"], ",", 
         RowBox[{
          SubscriptBox["\[Theta]", "2"], "[", "t", "]"}]}], "}"}], ",", 
       RowBox[{
        SubscriptBox["g", "hip"], "[", "0", "]"}]}], "]"}], "/.", 
     "constsubs"}], "]"}]}], ";"}], "\n", 
 RowBox[{
  RowBox[{
   RowBox[{
    SubscriptBox["g", "torso"], "[", "\[Theta]", "]"}], "=", 
   RowBox[{"Simplify", "[", 
    RowBox[{
     RowBox[{"ForwardKinematics", "[", 
      RowBox[{
       RowBox[{"{", 
        RowBox[{
         SubscriptBox["\[Xi]", "q1"], ",", 
         RowBox[{
          SubscriptBox["\[Theta]", "1"], "[", "t", "]"}]}], "}"}], ",", 
       RowBox[{"{", 
        RowBox[{
         SubscriptBox["\[Xi]", "q2"], ",", 
         RowBox[{
          SubscriptBox["\[Theta]", "2"], "[", "t", "]"}]}], "}"}], ",", 
       RowBox[{"{", 
        RowBox[{
         SubscriptBox["\[Xi]", "q3"], ",", 
         RowBox[{
          SubscriptBox["\[Theta]", "3"], "[", "t", "]"}]}], "}"}], ",", 
       RowBox[{
        SubscriptBox["g", "torso"], "[", "0", "]"}]}], "]"}], "/.", 
     "constsubs"}], "]"}]}], ";"}], "\[IndentingNewLine]", 
 RowBox[{
  RowBox[{
   RowBox[{
    RowBox[{
     SubscriptBox["g", "nsk"], "[", "\[Theta]", "]"}], "=", 
    RowBox[{"Simplify", "[", 
     RowBox[{
      RowBox[{"ForwardKinematics", "[", 
       RowBox[{
        RowBox[{"{", 
         RowBox[{
          SubscriptBox["\[Xi]", "q1"], ",", 
          RowBox[{
           SubscriptBox["\[Theta]", "1"], "[", "t", "]"}]}], "}"}], ",", 
        RowBox[{"{", 
         RowBox[{
          SubscriptBox["\[Xi]", "q2"], ",", 
          RowBox[{
           SubscriptBox["\[Theta]", "2"], "[", "t", "]"}]}], "}"}], ",", 
        RowBox[{"{", 
         RowBox[{
          SubscriptBox["\[Xi]", "q3"], ",", 
          RowBox[{
           SubscriptBox["\[Theta]", "3"], "[", "t", "]"}]}], "}"}], ",", 
        RowBox[{"{", 
         RowBox[{
          SubscriptBox["\[Xi]", "q4"], ",", 
          RowBox[{
           SubscriptBox["\[Theta]", "4"], "[", "t", "]"}]}], "}"}], ",", 
        RowBox[{
         SubscriptBox["g", "nsk"], "[", "0", "]"}]}], "]"}], "/.", 
      "constsubs"}], "]"}]}], ";", 
   RowBox[{
    RowBox[{
     SubscriptBox["g", "nsf"], "[", "\[Theta]", "]"}], "=", 
    RowBox[{"Simplify", "[", 
     RowBox[{
      RowBox[{"ForwardKinematics", "[", 
       RowBox[{
        RowBox[{"{", 
         RowBox[{
          SubscriptBox["\[Xi]", "q1"], ",", 
          RowBox[{
           SubscriptBox["\[Theta]", "1"], "[", "t", "]"}]}], "}"}], ",", 
        RowBox[{"{", 
         RowBox[{
          SubscriptBox["\[Xi]", "q2"], ",", 
          RowBox[{
           SubscriptBox["\[Theta]", "2"], "[", "t", "]"}]}], "}"}], ",", 
        RowBox[{"{", 
         RowBox[{
          SubscriptBox["\[Xi]", "q3"], ",", 
          RowBox[{
           SubscriptBox["\[Theta]", "3"], "[", "t", "]"}]}], "}"}], ",", 
        RowBox[{"{", 
         RowBox[{
          SubscriptBox["\[Xi]", "q4"], ",", 
          RowBox[{
           SubscriptBox["\[Theta]", "4"], "[", "t", "]"}]}], "}"}], ",", 
        RowBox[{"{", 
         RowBox[{
          SubscriptBox["\[Xi]", "q5"], ",", 
          RowBox[{
           SubscriptBox["\[Theta]", "5"], "[", "t", "]"}]}], "}"}], ",", 
        RowBox[{
         SubscriptBox["g", "nsf"], "[", "0", "]"}]}], "]"}], "/.", 
      "constsubs"}], "]"}]}], ";"}], "\[IndentingNewLine]", 
  RowBox[{"(*", "aniplot", "*)"}]}], "\n", 
 RowBox[{"pos", "=", 
  RowBox[{"Simplify", "[", 
   RowBox[{"Join", "[", 
    RowBox[{
     RowBox[{
      RowBox[{
       SubscriptBox["g", "stf"], "[", "\[Theta]", "]"}], 
      "\[LeftDoubleBracket]", 
      RowBox[{
       RowBox[{"{", 
        RowBox[{"1", ",", "3"}], "}"}], ",", 
       RowBox[{"{", "4", "}"}]}], "\[RightDoubleBracket]"}], ",", 
     RowBox[{
      RowBox[{
       SubscriptBox["g", "stk"], "[", "\[Theta]", "]"}], 
      "\[LeftDoubleBracket]", 
      RowBox[{
       RowBox[{"{", 
        RowBox[{"1", ",", "3"}], "}"}], ",", 
       RowBox[{"{", "4", "}"}]}], "\[RightDoubleBracket]"}], ",", 
     RowBox[{
      RowBox[{
       SubscriptBox["g", "hip"], "[", "\[Theta]", "]"}], 
      "\[LeftDoubleBracket]", 
      RowBox[{
       RowBox[{"{", 
        RowBox[{"1", ",", "3"}], "}"}], ",", 
       RowBox[{"{", "4", "}"}]}], "\[RightDoubleBracket]"}], ",", 
     RowBox[{
      RowBox[{
       SubscriptBox["g", "torso"], "[", "\[Theta]", "]"}], 
      "\[LeftDoubleBracket]", 
      RowBox[{
       RowBox[{"{", 
        RowBox[{"1", ",", "3"}], "}"}], ",", 
       RowBox[{"{", "4", "}"}]}], "\[RightDoubleBracket]"}], ",", 
     RowBox[{
      RowBox[{
       SubscriptBox["g", "hip"], "[", "\[Theta]", "]"}], 
      "\[LeftDoubleBracket]", 
      RowBox[{
       RowBox[{"{", 
        RowBox[{"1", ",", "3"}], "}"}], ",", 
       RowBox[{"{", "4", "}"}]}], "\[RightDoubleBracket]"}], ",", 
     RowBox[{
      RowBox[{
       SubscriptBox["g", "nsk"], "[", "\[Theta]", "]"}], 
      "\[LeftDoubleBracket]", 
      RowBox[{
       RowBox[{"{", 
        RowBox[{"1", ",", "3"}], "}"}], ",", 
       RowBox[{"{", "4", "}"}]}], "\[RightDoubleBracket]"}], ",", 
     RowBox[{
      RowBox[{
       SubscriptBox["g", "nsf"], "[", "\[Theta]", "]"}], 
      "\[LeftDoubleBracket]", 
      RowBox[{
       RowBox[{"{", 
        RowBox[{"1", ",", "3"}], "}"}], ",", 
       RowBox[{"{", "4", "}"}]}], "\[RightDoubleBracket]"}], ",", "2"}], 
    "]"}], "]"}]}]}], "Input",
 CellChangeTimes->{
  3.504875918224949*^9, {3.505063952502133*^9, 3.505063953970308*^9}, {
   3.5051489336912956`*^9, 3.5051489340032964`*^9}, 3.513531757509935*^9, {
   3.515870024222504*^9, 3.5158700252255616`*^9}, 3.5158863755610266`*^9, {
   3.5271753048660097`*^9, 3.527175316328361*^9}, {3.5271784037700644`*^9, 
   3.527178409550562*^9}, {3.527194806620797*^9, 3.527194812727977*^9}, {
   3.527194901003693*^9, 3.527194939760748*^9}, 3.527195000990027*^9, 
   3.527195065536849*^9, {3.527197843268285*^9, 3.527197851612855*^9}, {
   3.527266876626479*^9, 3.5272669091021013`*^9}, 3.5272680582479362`*^9, {
   3.52726826837751*^9, 3.5272682685913153`*^9}, {3.527268323380795*^9, 
   3.52726834320557*^9}, {3.527268721025804*^9, 3.5272687345158367`*^9}, 
   3.527271522279252*^9, {3.527280823762142*^9, 3.527280828414195*^9}, 
   3.5272808762225933`*^9, {3.527281589348708*^9, 3.52728158963411*^9}, 
   3.5272816841443167`*^9}],

Cell[BoxData[
 FormBox[
  RowBox[{"(", "\[NoBreak]", GridBox[{
     {"0", 
      RowBox[{"-", 
       FractionBox[
        RowBox[{"17399", " ", 
         RowBox[{"sin", "(", 
          RowBox[{
           SubscriptBox["\[Theta]", "1"], "(", "t", ")"}], ")"}]}], 
        "50000"]}], 
      RowBox[{"-", 
       FractionBox[
        RowBox[{"127", " ", 
         RowBox[{"(", 
          RowBox[{
           RowBox[{"685", " ", 
            RowBox[{"sin", "(", 
             RowBox[{
              SubscriptBox["\[Theta]", "1"], "(", "t", ")"}], ")"}]}], "+", 
           RowBox[{"514", " ", 
            RowBox[{"sin", "(", 
             RowBox[{
              RowBox[{
               SubscriptBox["\[Theta]", "1"], "(", "t", ")"}], "+", 
              RowBox[{
               SubscriptBox["\[Theta]", "2"], "(", "t", ")"}]}], ")"}]}]}], 
          ")"}]}], "250000"]}], 
      FractionBox[
       RowBox[{
        RowBox[{
         RowBox[{"-", "173990"}], " ", 
         RowBox[{"sin", "(", 
          RowBox[{
           SubscriptBox["\[Theta]", "1"], "(", "t", ")"}], ")"}]}], "-", 
        RowBox[{"130556", " ", 
         RowBox[{"sin", "(", 
          RowBox[{
           RowBox[{
            SubscriptBox["\[Theta]", "1"], "(", "t", ")"}], "+", 
           RowBox[{
            SubscriptBox["\[Theta]", "2"], "(", "t", ")"}]}], ")"}]}], "-", 
        RowBox[{"4985", " ", 
         RowBox[{"sin", "(", 
          RowBox[{
           RowBox[{
            SubscriptBox["\[Theta]", "1"], "(", "t", ")"}], "+", 
           RowBox[{
            SubscriptBox["\[Theta]", "2"], "(", "t", ")"}], "+", 
           RowBox[{
            SubscriptBox["\[Theta]", "3"], "(", "t", ")"}]}], ")"}]}]}], 
       "500000"], 
      RowBox[{"-", 
       FractionBox[
        RowBox[{"127", " ", 
         RowBox[{"(", 
          RowBox[{
           RowBox[{"685", " ", 
            RowBox[{"sin", "(", 
             RowBox[{
              SubscriptBox["\[Theta]", "1"], "(", "t", ")"}], ")"}]}], "+", 
           RowBox[{"514", " ", 
            RowBox[{"sin", "(", 
             RowBox[{
              RowBox[{
               SubscriptBox["\[Theta]", "1"], "(", "t", ")"}], "+", 
              RowBox[{
               SubscriptBox["\[Theta]", "2"], "(", "t", ")"}]}], ")"}]}]}], 
          ")"}]}], "250000"]}], 
      FractionBox[
       RowBox[{"127", " ", 
        RowBox[{"(", 
         RowBox[{
          RowBox[{
           RowBox[{"-", "685"}], " ", 
           RowBox[{"sin", "(", 
            RowBox[{
             SubscriptBox["\[Theta]", "1"], "(", "t", ")"}], ")"}]}], "-", 
          RowBox[{"514", " ", 
           RowBox[{"(", 
            RowBox[{
             RowBox[{"sin", "(", 
              RowBox[{
               RowBox[{
                SubscriptBox["\[Theta]", "1"], "(", "t", ")"}], "+", 
               RowBox[{
                SubscriptBox["\[Theta]", "2"], "(", "t", ")"}]}], ")"}], "-", 
             
             RowBox[{"sin", "(", 
              RowBox[{
               RowBox[{
                SubscriptBox["\[Theta]", "1"], "(", "t", ")"}], "+", 
               RowBox[{
                SubscriptBox["\[Theta]", "2"], "(", "t", ")"}], "+", 
               RowBox[{
                SubscriptBox["\[Theta]", "3"], "(", "t", ")"}], "-", 
               RowBox[{
                SubscriptBox["\[Theta]", "4"], "(", "t", ")"}]}], ")"}]}], 
            ")"}]}]}], ")"}]}], "250000"], 
      FractionBox[
       RowBox[{"127", " ", 
        RowBox[{"(", 
         RowBox[{
          RowBox[{
           RowBox[{"-", "685"}], " ", 
           RowBox[{"sin", "(", 
            RowBox[{
             SubscriptBox["\[Theta]", "1"], "(", "t", ")"}], ")"}]}], "-", 
          RowBox[{"514", " ", 
           RowBox[{"sin", "(", 
            RowBox[{
             RowBox[{
              SubscriptBox["\[Theta]", "1"], "(", "t", ")"}], "+", 
             RowBox[{
              SubscriptBox["\[Theta]", "2"], "(", "t", ")"}]}], ")"}]}], "+", 
          
          RowBox[{"514", " ", 
           RowBox[{"sin", "(", 
            RowBox[{
             RowBox[{
              SubscriptBox["\[Theta]", "1"], "(", "t", ")"}], "+", 
             RowBox[{
              SubscriptBox["\[Theta]", "2"], "(", "t", ")"}], "+", 
             RowBox[{
              SubscriptBox["\[Theta]", "3"], "(", "t", ")"}], "-", 
             RowBox[{
              SubscriptBox["\[Theta]", "4"], "(", "t", ")"}]}], ")"}]}], "+", 
          
          RowBox[{"685", " ", 
           RowBox[{"sin", "(", 
            RowBox[{
             RowBox[{
              SubscriptBox["\[Theta]", "1"], "(", "t", ")"}], "+", 
             RowBox[{
              SubscriptBox["\[Theta]", "2"], "(", "t", ")"}], "+", 
             RowBox[{
              SubscriptBox["\[Theta]", "3"], "(", "t", ")"}], "-", 
             RowBox[{
              SubscriptBox["\[Theta]", "4"], "(", "t", ")"}], "-", 
             RowBox[{
              SubscriptBox["\[Theta]", "5"], "(", "t", ")"}]}], ")"}]}]}], 
         ")"}]}], "250000"]},
     {"0", 
      FractionBox[
       RowBox[{"17399", " ", 
        RowBox[{"cos", "(", 
         RowBox[{
          SubscriptBox["\[Theta]", "1"], "(", "t", ")"}], ")"}]}], "50000"], 
      FractionBox[
       RowBox[{"127", " ", 
        RowBox[{"(", 
         RowBox[{
          RowBox[{"685", " ", 
           RowBox[{"cos", "(", 
            RowBox[{
             SubscriptBox["\[Theta]", "1"], "(", "t", ")"}], ")"}]}], "+", 
          RowBox[{"514", " ", 
           RowBox[{"cos", "(", 
            RowBox[{
             RowBox[{
              SubscriptBox["\[Theta]", "1"], "(", "t", ")"}], "+", 
             RowBox[{
              SubscriptBox["\[Theta]", "2"], "(", "t", ")"}]}], ")"}]}]}], 
         ")"}]}], "250000"], 
      FractionBox[
       RowBox[{
        RowBox[{"173990", " ", 
         RowBox[{"cos", "(", 
          RowBox[{
           SubscriptBox["\[Theta]", "1"], "(", "t", ")"}], ")"}]}], "+", 
        RowBox[{"130556", " ", 
         RowBox[{"cos", "(", 
          RowBox[{
           RowBox[{
            SubscriptBox["\[Theta]", "1"], "(", "t", ")"}], "+", 
           RowBox[{
            SubscriptBox["\[Theta]", "2"], "(", "t", ")"}]}], ")"}]}], "+", 
        RowBox[{"4985", " ", 
         RowBox[{"cos", "(", 
          RowBox[{
           RowBox[{
            SubscriptBox["\[Theta]", "1"], "(", "t", ")"}], "+", 
           RowBox[{
            SubscriptBox["\[Theta]", "2"], "(", "t", ")"}], "+", 
           RowBox[{
            SubscriptBox["\[Theta]", "3"], "(", "t", ")"}]}], ")"}]}]}], 
       "500000"], 
      FractionBox[
       RowBox[{"127", " ", 
        RowBox[{"(", 
         RowBox[{
          RowBox[{"685", " ", 
           RowBox[{"cos", "(", 
            RowBox[{
             SubscriptBox["\[Theta]", "1"], "(", "t", ")"}], ")"}]}], "+", 
          RowBox[{"514", " ", 
           RowBox[{"cos", "(", 
            RowBox[{
             RowBox[{
              SubscriptBox["\[Theta]", "1"], "(", "t", ")"}], "+", 
             RowBox[{
              SubscriptBox["\[Theta]", "2"], "(", "t", ")"}]}], ")"}]}]}], 
         ")"}]}], "250000"], 
      FractionBox[
       RowBox[{"127", " ", 
        RowBox[{"(", 
         RowBox[{
          RowBox[{"685", " ", 
           RowBox[{"cos", "(", 
            RowBox[{
             SubscriptBox["\[Theta]", "1"], "(", "t", ")"}], ")"}]}], "+", 
          RowBox[{"514", " ", 
           RowBox[{"(", 
            RowBox[{
             RowBox[{"cos", "(", 
              RowBox[{
               RowBox[{
                SubscriptBox["\[Theta]", "1"], "(", "t", ")"}], "+", 
               RowBox[{
                SubscriptBox["\[Theta]", "2"], "(", "t", ")"}]}], ")"}], "-", 
             
             RowBox[{"cos", "(", 
              RowBox[{
               RowBox[{
                SubscriptBox["\[Theta]", "1"], "(", "t", ")"}], "+", 
               RowBox[{
                SubscriptBox["\[Theta]", "2"], "(", "t", ")"}], "+", 
               RowBox[{
                SubscriptBox["\[Theta]", "3"], "(", "t", ")"}], "-", 
               RowBox[{
                SubscriptBox["\[Theta]", "4"], "(", "t", ")"}]}], ")"}]}], 
            ")"}]}]}], ")"}]}], "250000"], 
      FractionBox[
       RowBox[{"127", " ", 
        RowBox[{"(", 
         RowBox[{
          RowBox[{"685", " ", 
           RowBox[{"cos", "(", 
            RowBox[{
             SubscriptBox["\[Theta]", "1"], "(", "t", ")"}], ")"}]}], "+", 
          RowBox[{"514", " ", 
           RowBox[{"cos", "(", 
            RowBox[{
             RowBox[{
              SubscriptBox["\[Theta]", "1"], "(", "t", ")"}], "+", 
             RowBox[{
              SubscriptBox["\[Theta]", "2"], "(", "t", ")"}]}], ")"}]}], "-", 
          
          RowBox[{"514", " ", 
           RowBox[{"cos", "(", 
            RowBox[{
             RowBox[{
              SubscriptBox["\[Theta]", "1"], "(", "t", ")"}], "+", 
             RowBox[{
              SubscriptBox["\[Theta]", "2"], "(", "t", ")"}], "+", 
             RowBox[{
              SubscriptBox["\[Theta]", "3"], "(", "t", ")"}], "-", 
             RowBox[{
              SubscriptBox["\[Theta]", "4"], "(", "t", ")"}]}], ")"}]}], "-", 
          
          RowBox[{"685", " ", 
           RowBox[{"cos", "(", 
            RowBox[{
             RowBox[{
              SubscriptBox["\[Theta]", "1"], "(", "t", ")"}], "+", 
             RowBox[{
              SubscriptBox["\[Theta]", "2"], "(", "t", ")"}], "+", 
             RowBox[{
              SubscriptBox["\[Theta]", "3"], "(", "t", ")"}], "-", 
             RowBox[{
              SubscriptBox["\[Theta]", "4"], "(", "t", ")"}], "-", 
             RowBox[{
              SubscriptBox["\[Theta]", "5"], "(", "t", ")"}]}], ")"}]}]}], 
         ")"}]}], "250000"]}
    },
    GridBoxAlignment->{
     "Columns" -> {{Center}}, "ColumnsIndexed" -> {}, "Rows" -> {{Baseline}}, 
      "RowsIndexed" -> {}},
    GridBoxSpacings->{"Columns" -> {
        Offset[0.27999999999999997`], {
         Offset[0.7]}, 
        Offset[0.27999999999999997`]}, "ColumnsIndexed" -> {}, "Rows" -> {
        Offset[0.2], {
         Offset[0.4]}, 
        Offset[0.2]}, "RowsIndexed" -> {}}], "\[NoBreak]", ")"}], 
  TraditionalForm]], "Output",
 CellChangeTimes->{3.5272808786862173`*^9, 3.52728160990996*^9, 
  3.527281936460037*^9}]
}, Open  ]],

Cell[CellGroupData[{

Cell[BoxData[
 RowBox[{
  SubscriptBox["g", "torso"], "[", "\[Theta]", "]"}]], "Input"],

Cell[BoxData[
 FormBox[
  RowBox[{"(", "\[NoBreak]", GridBox[{
     {
      RowBox[{"cos", "(", 
       RowBox[{
        RowBox[{
         SubscriptBox["\[Theta]", "1"], "(", "t", ")"}], "+", 
        RowBox[{
         SubscriptBox["\[Theta]", "2"], "(", "t", ")"}], "+", 
        RowBox[{
         SubscriptBox["\[Theta]", "3"], "(", "t", ")"}]}], ")"}], "0", 
      RowBox[{"-", 
       RowBox[{"sin", "(", 
        RowBox[{
         RowBox[{
          SubscriptBox["\[Theta]", "1"], "(", "t", ")"}], "+", 
         RowBox[{
          SubscriptBox["\[Theta]", "2"], "(", "t", ")"}], "+", 
         RowBox[{
          SubscriptBox["\[Theta]", "3"], "(", "t", ")"}]}], ")"}]}], 
      FractionBox[
       RowBox[{
        RowBox[{
         RowBox[{"-", "173990"}], " ", 
         RowBox[{"sin", "(", 
          RowBox[{
           SubscriptBox["\[Theta]", "1"], "(", "t", ")"}], ")"}]}], "-", 
        RowBox[{"130556", " ", 
         RowBox[{"sin", "(", 
          RowBox[{
           RowBox[{
            SubscriptBox["\[Theta]", "1"], "(", "t", ")"}], "+", 
           RowBox[{
            SubscriptBox["\[Theta]", "2"], "(", "t", ")"}]}], ")"}]}], "-", 
        RowBox[{"4985", " ", 
         RowBox[{"sin", "(", 
          RowBox[{
           RowBox[{
            SubscriptBox["\[Theta]", "1"], "(", "t", ")"}], "+", 
           RowBox[{
            SubscriptBox["\[Theta]", "2"], "(", "t", ")"}], "+", 
           RowBox[{
            SubscriptBox["\[Theta]", "3"], "(", "t", ")"}]}], ")"}]}]}], 
       "500000"]},
     {"0", "1", "0", "0"},
     {
      RowBox[{"sin", "(", 
       RowBox[{
        RowBox[{
         SubscriptBox["\[Theta]", "1"], "(", "t", ")"}], "+", 
        RowBox[{
         SubscriptBox["\[Theta]", "2"], "(", "t", ")"}], "+", 
        RowBox[{
         SubscriptBox["\[Theta]", "3"], "(", "t", ")"}]}], ")"}], "0", 
      RowBox[{"cos", "(", 
       RowBox[{
        RowBox[{
         SubscriptBox["\[Theta]", "1"], "(", "t", ")"}], "+", 
        RowBox[{
         SubscriptBox["\[Theta]", "2"], "(", "t", ")"}], "+", 
        RowBox[{
         SubscriptBox["\[Theta]", "3"], "(", "t", ")"}]}], ")"}], 
      FractionBox[
       RowBox[{
        RowBox[{"173990", " ", 
         RowBox[{"cos", "(", 
          RowBox[{
           SubscriptBox["\[Theta]", "1"], "(", "t", ")"}], ")"}]}], "+", 
        RowBox[{"130556", " ", 
         RowBox[{"cos", "(", 
          RowBox[{
           RowBox[{
            SubscriptBox["\[Theta]", "1"], "(", "t", ")"}], "+", 
           RowBox[{
            SubscriptBox["\[Theta]", "2"], "(", "t", ")"}]}], ")"}]}], "+", 
        RowBox[{"4985", " ", 
         RowBox[{"cos", "(", 
          RowBox[{
           RowBox[{
            SubscriptBox["\[Theta]", "1"], "(", "t", ")"}], "+", 
           RowBox[{
            SubscriptBox["\[Theta]", "2"], "(", "t", ")"}], "+", 
           RowBox[{
            SubscriptBox["\[Theta]", "3"], "(", "t", ")"}]}], ")"}]}]}], 
       "500000"]},
     {"0", "0", "0", "1"}
    },
    GridBoxAlignment->{
     "Columns" -> {{Center}}, "ColumnsIndexed" -> {}, "Rows" -> {{Baseline}}, 
      "RowsIndexed" -> {}},
    GridBoxSpacings->{"Columns" -> {
        Offset[0.27999999999999997`], {
         Offset[0.7]}, 
        Offset[0.27999999999999997`]}, "ColumnsIndexed" -> {}, "Rows" -> {
        Offset[0.2], {
         Offset[0.4]}, 
        Offset[0.2]}, "RowsIndexed" -> {}}], "\[NoBreak]", ")"}], 
  TraditionalForm]], "Output",
 CellChangeTimes->{3.52726874563942*^9, 3.527278886611117*^9, 
  3.527281936522841*^9}]
}, Open  ]],

Cell[CellGroupData[{

Cell[BoxData[
 RowBox[{
  SubscriptBox["g", "hip"], "[", "\[Theta]", "]"}]], "Input",
 CellChangeTimes->{{3.52726875328576*^9, 3.527268754810855*^9}}],

Cell[BoxData[
 FormBox[
  RowBox[{"(", "\[NoBreak]", GridBox[{
     {
      RowBox[{"cos", "(", 
       RowBox[{
        RowBox[{
         SubscriptBox["\[Theta]", "1"], "(", "t", ")"}], "+", 
        RowBox[{
         SubscriptBox["\[Theta]", "2"], "(", "t", ")"}]}], ")"}], "0", 
      RowBox[{"-", 
       RowBox[{"sin", "(", 
        RowBox[{
         RowBox[{
          SubscriptBox["\[Theta]", "1"], "(", "t", ")"}], "+", 
         RowBox[{
          SubscriptBox["\[Theta]", "2"], "(", "t", ")"}]}], ")"}]}], 
      RowBox[{"-", 
       FractionBox[
        RowBox[{"127", " ", 
         RowBox[{"(", 
          RowBox[{
           RowBox[{"685", " ", 
            RowBox[{"sin", "(", 
             RowBox[{
              SubscriptBox["\[Theta]", "1"], "(", "t", ")"}], ")"}]}], "+", 
           RowBox[{"514", " ", 
            RowBox[{"sin", "(", 
             RowBox[{
              RowBox[{
               SubscriptBox["\[Theta]", "1"], "(", "t", ")"}], "+", 
              RowBox[{
               SubscriptBox["\[Theta]", "2"], "(", "t", ")"}]}], ")"}]}]}], 
          ")"}]}], "250000"]}]},
     {"0", "1", "0", "0"},
     {
      RowBox[{"sin", "(", 
       RowBox[{
        RowBox[{
         SubscriptBox["\[Theta]", "1"], "(", "t", ")"}], "+", 
        RowBox[{
         SubscriptBox["\[Theta]", "2"], "(", "t", ")"}]}], ")"}], "0", 
      RowBox[{"cos", "(", 
       RowBox[{
        RowBox[{
         SubscriptBox["\[Theta]", "1"], "(", "t", ")"}], "+", 
        RowBox[{
         SubscriptBox["\[Theta]", "2"], "(", "t", ")"}]}], ")"}], 
      FractionBox[
       RowBox[{"127", " ", 
        RowBox[{"(", 
         RowBox[{
          RowBox[{"685", " ", 
           RowBox[{"cos", "(", 
            RowBox[{
             SubscriptBox["\[Theta]", "1"], "(", "t", ")"}], ")"}]}], "+", 
          RowBox[{"514", " ", 
           RowBox[{"cos", "(", 
            RowBox[{
             RowBox[{
              SubscriptBox["\[Theta]", "1"], "(", "t", ")"}], "+", 
             RowBox[{
              SubscriptBox["\[Theta]", "2"], "(", "t", ")"}]}], ")"}]}]}], 
         ")"}]}], "250000"]},
     {"0", "0", "0", "1"}
    },
    GridBoxAlignment->{
     "Columns" -> {{Center}}, "ColumnsIndexed" -> {}, "Rows" -> {{Baseline}}, 
      "RowsIndexed" -> {}},
    GridBoxSpacings->{"Columns" -> {
        Offset[0.27999999999999997`], {
         Offset[0.7]}, 
        Offset[0.27999999999999997`]}, "ColumnsIndexed" -> {}, "Rows" -> {
        Offset[0.2], {
         Offset[0.4]}, 
        Offset[0.2]}, "RowsIndexed" -> {}}], "\[NoBreak]", ")"}], 
  TraditionalForm]], "Output",
 CellChangeTimes->{3.5272687552712708`*^9, 3.527278886831811*^9, 
  3.527281936581562*^9}]
}, Open  ]],

Cell[CellGroupData[{

Cell[BoxData[
 RowBox[{"\n", 
  RowBox[{"(*", 
   RowBox[{"calculate", " ", "center", " ", "of", " ", "mass"}], "*)"}], 
  "\[IndentingNewLine]", 
  RowBox[{
   RowBox[{
    SubscriptBox["p", "COM"], "=", 
    RowBox[{
     RowBox[{
      RowBox[{
       RowBox[{"(", 
        RowBox[{
         UnderoverscriptBox["\[Sum]", 
          RowBox[{"i", "=", "1"}], 
          RowBox[{"Length", "[", "mm", "]"}]], 
         RowBox[{
          RowBox[{
          "mm", "\[LeftDoubleBracket]", "i", "\[RightDoubleBracket]"}], " ", 
          RowBox[{
           RowBox[{
            RowBox[{
             SubscriptBox["g", "i"], "[", "\[Theta]", "]"}], 
            "\[LeftDoubleBracket]", 
            RowBox[{"1", ",", "4"}], "\[RightDoubleBracket]"}], "/", 
           RowBox[{"(", 
            RowBox[{"Total", "[", "mm", "]"}], ")"}]}]}]}], ")"}], "/.", 
       "constsubs"}], "//.", "p0"}], "//", "Simplify"}]}], 
   "\[IndentingNewLine]", 
   RowBox[{"(*", 
    RowBox[{
    "calculate", " ", "the", " ", "manipulator", " ", "inertia", " ", 
     "matrix"}], "*)"}], "\n", 
   RowBox[{
    RowBox[{"\[ScriptCapitalD]e", "=", 
     RowBox[{"Simplify", "[", 
      RowBox[{
       UnderoverscriptBox["\[Sum]", 
        RowBox[{"i", "=", "1"}], 
        RowBox[{"Length", "[", "mm", "]"}]], 
       RowBox[{
        RowBox[{
         SubscriptBox["\[ScriptCapitalJ]", "i"], "\[Transpose]"}], ".", 
        SubscriptBox["\[ScriptCapitalM]", "i"], ".", 
        SubscriptBox["\[ScriptCapitalJ]", "i"]}]}], "]"}]}], ";"}], 
   "\[IndentingNewLine]", 
   RowBox[{"(*", 
    RowBox[{
    "project", " ", "out", " ", "the", " ", "generalized", " ", "coordinates",
      " ", "defining", " ", "the", " ", "position", " ", "of", " ", "the", 
     " ", "stance", " ", "foot", " ", "to", " ", "obtain", " ", "the", " ", 
     "reduced", " ", "\[ScriptCapitalD]", " ", "matrix"}], "*)"}], "\n", 
   RowBox[{
    RowBox[{"\[ScriptCapitalD]", "=", 
     RowBox[{"Simplify", "[", 
      RowBox[{
       RowBox[{"\[ScriptCapitalD]e", "\[LeftDoubleBracket]", 
        RowBox[{
         RowBox[{"3", ";;", "All"}], ",", 
         RowBox[{"3", ";;", "All"}]}], "\[RightDoubleBracket]"}], "/.", 
       "\[InvisibleSpace]", "p0"}], "]"}]}], ";"}], "\n", 
   RowBox[{"(*", 
    RowBox[{"calculate", " ", "Coriolis", " ", "matrix"}], "*)"}], 
   "\[IndentingNewLine]", 
   RowBox[{
    RowBox[{"\[ScriptCapitalC]", "=", 
     RowBox[{"Simplify", "[", 
      RowBox[{"InertiaToCoriolis", "[", 
       RowBox[{"\[ScriptCapitalD]", ",", 
        RowBox[{"Flatten", "[", "q", "]"}], ",", 
        RowBox[{"Flatten", "[", "dq", "]"}]}], "]"}], "]"}]}], ";"}], "\n", 
   RowBox[{"(*", 
    RowBox[{
    "calculate", " ", "the", " ", "potential", " ", "energy", " ", "and", " ",
      "\[ScriptCapitalG]", " ", "matrix"}], "*)"}], "\[IndentingNewLine]", 
   RowBox[{
    RowBox[{"V", "=", 
     RowBox[{"Simplify", "[", 
      RowBox[{
       RowBox[{"g", " ", 
        RowBox[{
         UnderoverscriptBox["\[Sum]", 
          RowBox[{"i", "=", "1"}], 
          RowBox[{"Length", "[", "mm", "]"}]], 
         RowBox[{
          RowBox[{
          "mm", "\[LeftDoubleBracket]", "i", "\[RightDoubleBracket]"}], " ", 
          RowBox[{
           RowBox[{
            SubscriptBox["g", "i"], "[", "\[Theta]", "]"}], 
           "\[LeftDoubleBracket]", 
           RowBox[{"3", ",", "4"}], "\[RightDoubleBracket]"}]}]}]}], "/.", 
       "\[InvisibleSpace]", "constsubs"}], "]"}]}], ";"}], "\n", 
   RowBox[{
    RowBox[{"\[ScriptCapitalG]", "=", 
     RowBox[{"Simplify", "[", 
      RowBox[{
       SubscriptBox["\[PartialD]", 
        RowBox[{"{", 
         RowBox[{"q", ",", "1"}], "}"}]], "V"}], "]"}]}], ";"}], "\n", 
   RowBox[{"(*", 
    RowBox[{
    "calculate", " ", "the", " ", "\[ScriptCapitalE]", " ", "matrix", " ", 
     "for", " ", "impact", " ", "and", " ", "the", " ", "guard"}], "*)"}], 
   "\[IndentingNewLine]", 
   RowBox[{"(*", 
    RowBox[{"the", " ", "position", " ", "of", " ", "swing", " ", "foot"}], 
    "*)"}], "\[IndentingNewLine]", 
   RowBox[{
    RowBox[{
     RowBox[{
      SubscriptBox["g", "nsf"], "[", "0", "]"}], "=", 
     RowBox[{"RPToHomogeneous", "[", 
      RowBox[{
       RowBox[{"IdentityMatrix", "[", "3", "]"}], ",", 
       RowBox[{"{", 
        RowBox[{"0", ",", "0", ",", "0"}], "}"}]}], "]"}]}], ";"}], "\n", 
   RowBox[{
    RowBox[{
     RowBox[{
      SubscriptBox["g", "nsf"], "[", "\[Theta]", "]"}], "=", 
     RowBox[{"Simplify", "[", 
      RowBox[{"ForwardKinematics", "[", 
       RowBox[{
        RowBox[{"{", 
         RowBox[{
          SubscriptBox["\[Xi]", "px"], ",", 
          RowBox[{
           SubscriptBox["p", "x"], "[", "t", "]"}]}], "}"}], ",", 
        RowBox[{"{", 
         RowBox[{
          SubscriptBox["\[Xi]", "pz"], ",", 
          RowBox[{
           SubscriptBox["p", "z"], "[", "t", "]"}]}], "}"}], ",", 
        RowBox[{"{", 
         RowBox[{
          SubscriptBox["\[Xi]", "q1"], ",", 
          RowBox[{
           SubscriptBox["\[Theta]", "1"], "[", "t", "]"}]}], "}"}], ",", 
        RowBox[{"{", 
         RowBox[{
          SubscriptBox["\[Xi]", "q2"], ",", 
          RowBox[{
           SubscriptBox["\[Theta]", "2"], "[", "t", "]"}]}], "}"}], ",", 
        RowBox[{"{", 
         RowBox[{
          SubscriptBox["\[Xi]", "q3"], ",", 
          RowBox[{
           SubscriptBox["\[Theta]", "3"], "[", "t", "]"}]}], "}"}], ",", 
        RowBox[{"{", 
         RowBox[{
          SubscriptBox["\[Xi]", "q4"], ",", 
          RowBox[{
           SubscriptBox["\[Theta]", "4"], "[", "t", "]"}]}], "}"}], ",", 
        RowBox[{"{", 
         RowBox[{
          SubscriptBox["\[Xi]", "q5"], ",", 
          RowBox[{
           SubscriptBox["\[Theta]", "5"], "[", "t", "]"}]}], "}"}], ",", 
        RowBox[{
         SubscriptBox["g", "nsf"], "[", "0", "]"}]}], "]"}], "]"}]}], ";"}], 
   "\n", 
   RowBox[{
    RowBox[{"\[ScriptCapitalE]", "=", 
     RowBox[{"Simplify", "[", 
      RowBox[{
       SubscriptBox["\[PartialD]", 
        RowBox[{"{", 
         RowBox[{
          RowBox[{"Flatten", "[", "qe", "]"}], ",", "1"}], "}"}]], 
       RowBox[{
        RowBox[{
         SubscriptBox["g", "nsf"], "[", "\[Theta]", "]"}], 
        "\[LeftDoubleBracket]", 
        RowBox[{
         RowBox[{"{", 
          RowBox[{"1", ",", "3"}], "}"}], ",", "4"}], 
        "\[RightDoubleBracket]"}]}], "]"}]}], ";"}], "\n", 
   RowBox[{"\[ScriptCapitalE]", "/.", 
    RowBox[{"{", 
     RowBox[{
      RowBox[{
       RowBox[{
        SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "\[Rule]", ".2345"}], 
      ",", 
      RowBox[{
       RowBox[{
        SubscriptBox["\[Theta]", "2"], "[", "t", "]"}], "\[Rule]", ".1894"}], 
      ",", 
      RowBox[{
       RowBox[{
        SubscriptBox["\[Theta]", "3"], "[", "t", "]"}], "\[Rule]", 
       RowBox[{"-", ".293"}]}], ",", 
      RowBox[{
       RowBox[{
        SubscriptBox["\[Theta]", "4"], "[", "t", "]"}], "\[Rule]", ".094"}], 
      ",", 
      RowBox[{
       RowBox[{
        SubscriptBox["\[Theta]", "4"], "[", "t", "]"}], "\[Rule]", 
       RowBox[{"-", ".210"}]}], ",", 
      RowBox[{
       RowBox[{
        SubscriptBox["\[Theta]", "5"], "[", "t", "]"}], "\[Rule]", ".923"}]}],
      "}"}]}]}]}]], "Input",
 CellChangeTimes->{
  3.504875918224949*^9, {3.505063952502133*^9, 3.505063953970308*^9}, {
   3.5051489336912956`*^9, 3.5051489340032964`*^9}, 3.513531757509935*^9, {
   3.515870024222504*^9, 3.5158700252255616`*^9}, 3.5158863755610266`*^9, {
   3.5271753048660097`*^9, 3.527175316328361*^9}, {3.5271784037700644`*^9, 
   3.527178409550562*^9}, {3.527194806620797*^9, 3.527194812727977*^9}, {
   3.527194901003693*^9, 3.527194939760748*^9}, 3.527195000990027*^9, 
   3.527195065536849*^9, {3.527197843268285*^9, 3.527197851612855*^9}, {
   3.527266876626479*^9, 3.5272669091021013`*^9}, 3.5272680582479362`*^9, {
   3.52726826837751*^9, 3.5272682685913153`*^9}, {3.527268323380795*^9, 
   3.52726834320557*^9}, {3.527268721025804*^9, 3.527268740044486*^9}, {
   3.527281326545467*^9, 3.527281336017424*^9}, 3.527281656208846*^9}],

Cell[BoxData[
 FormBox[
  RowBox[{
   FractionBox["1", "122235500000"], 
   RowBox[{"(", 
    RowBox[{
     RowBox[{"-", 
      RowBox[{"41834171195", " ", 
       RowBox[{"sin", "(", 
        RowBox[{
         SubscriptBox["\[Theta]", "1"], "(", "t", ")"}], ")"}]}]}], "-", 
     RowBox[{"25096253337", " ", 
      RowBox[{"sin", "(", 
       RowBox[{
        RowBox[{
         SubscriptBox["\[Theta]", "1"], "(", "t", ")"}], "+", 
        RowBox[{
         SubscriptBox["\[Theta]", "2"], "(", "t", ")"}]}], ")"}]}], "-", 
     RowBox[{"401207755", " ", 
      RowBox[{"sin", "(", 
       RowBox[{
        RowBox[{
         SubscriptBox["\[Theta]", "1"], "(", "t", ")"}], "+", 
        RowBox[{
         SubscriptBox["\[Theta]", "2"], "(", "t", ")"}], "+", 
        RowBox[{
         SubscriptBox["\[Theta]", "3"], "(", "t", ")"}]}], ")"}]}], "+", 
     RowBox[{"6820902539", " ", 
      RowBox[{"sin", "(", 
       RowBox[{
        RowBox[{
         SubscriptBox["\[Theta]", "1"], "(", "t", ")"}], "+", 
        RowBox[{
         SubscriptBox["\[Theta]", "2"], "(", "t", ")"}], "+", 
        RowBox[{
         SubscriptBox["\[Theta]", "3"], "(", "t", ")"}], "-", 
        RowBox[{
         SubscriptBox["\[Theta]", "4"], "(", "t", ")"}]}], ")"}]}], "+", 
     RowBox[{"701338095", " ", 
      RowBox[{"sin", "(", 
       RowBox[{
        RowBox[{
         SubscriptBox["\[Theta]", "1"], "(", "t", ")"}], "+", 
        RowBox[{
         SubscriptBox["\[Theta]", "2"], "(", "t", ")"}], "+", 
        RowBox[{
         SubscriptBox["\[Theta]", "3"], "(", "t", ")"}], "-", 
        RowBox[{
         SubscriptBox["\[Theta]", "4"], "(", "t", ")"}], "-", 
        RowBox[{
         SubscriptBox["\[Theta]", "5"], "(", "t", ")"}]}], ")"}]}], "+", 
     RowBox[{"606150", " ", 
      RowBox[{"cos", "(", 
       RowBox[{
        RowBox[{
         SubscriptBox["\[Theta]", "1"], "(", "t", ")"}], "+", 
        RowBox[{
         SubscriptBox["\[Theta]", "2"], "(", "t", ")"}]}], ")"}]}], "-", 
     RowBox[{"9657960", " ", 
      RowBox[{"cos", "(", 
       RowBox[{
        RowBox[{
         SubscriptBox["\[Theta]", "1"], "(", "t", ")"}], "+", 
        RowBox[{
         SubscriptBox["\[Theta]", "2"], "(", "t", ")"}], "+", 
        RowBox[{
         SubscriptBox["\[Theta]", "3"], "(", "t", ")"}]}], ")"}]}], "+", 
     RowBox[{"606150", " ", 
      RowBox[{"cos", "(", 
       RowBox[{
        RowBox[{
         SubscriptBox["\[Theta]", "1"], "(", "t", ")"}], "+", 
        RowBox[{
         SubscriptBox["\[Theta]", "2"], "(", "t", ")"}], "+", 
        RowBox[{
         SubscriptBox["\[Theta]", "3"], "(", "t", ")"}], "-", 
        RowBox[{
         SubscriptBox["\[Theta]", "4"], "(", "t", ")"}]}], ")"}]}]}], ")"}]}],
   TraditionalForm]], "Output",
 CellChangeTimes->{3.527281355612021*^9, 3.527281622898267*^9, 
  3.52728165661593*^9, 3.527281936821814*^9}],

Cell[BoxData[
 FormBox[
  RowBox[{"(", "\[NoBreak]", GridBox[{
     {"1", "0", 
      RowBox[{"-", "0.09544747668836438`"}], "0.2430085338559884`", 
      "0.4810099636462693`", 
      RowBox[{"-", "0.4810099636462693`"}], 
      RowBox[{"-", "0.22007570983164088`"}]},
     {"0", "1", 
      RowBox[{"-", "0.44817198873510994`"}], 
      RowBox[{"-", "0.36731650452778486`"}], 
      RowBox[{"-", "0.25991633710807366`"}], "0.25991633710807366`", 
      "0.26954918353076013`"}
    },
    GridBoxAlignment->{
     "Columns" -> {{Center}}, "ColumnsIndexed" -> {}, "Rows" -> {{Baseline}}, 
      "RowsIndexed" -> {}},
    GridBoxSpacings->{"Columns" -> {
        Offset[0.27999999999999997`], {
         Offset[0.7]}, 
        Offset[0.27999999999999997`]}, "ColumnsIndexed" -> {}, "Rows" -> {
        Offset[0.2], {
         Offset[0.4]}, 
        Offset[0.2]}, "RowsIndexed" -> {}}], "\[NoBreak]", ")"}], 
  TraditionalForm]], "Output",
 CellChangeTimes->{3.527281355612021*^9, 3.527281622898267*^9, 
  3.52728165661593*^9, 3.5272819451385717`*^9}]
}, Open  ]],

Cell[CellGroupData[{

Cell[BoxData["\[ScriptCapitalC]"], "Input"],

Cell[BoxData[
 FormBox[
  RowBox[{"(", "\[NoBreak]", GridBox[{
     {
      FractionBox[
       RowBox[{"127", " ", 
        RowBox[{"(", 
         RowBox[{
          RowBox[{"685", " ", 
           RowBox[{"(", 
            RowBox[{
             RowBox[{"606150", " ", 
              RowBox[{"cos", "(", 
               RowBox[{
                SubscriptBox["\[Theta]", "2"], "(", "t", ")"}], ")"}]}], "-", 
             
             RowBox[{"9657960", " ", 
              RowBox[{"cos", "(", 
               RowBox[{
                RowBox[{
                 SubscriptBox["\[Theta]", "2"], "(", "t", ")"}], "+", 
                RowBox[{
                 SubscriptBox["\[Theta]", "3"], "(", "t", ")"}]}], ")"}]}], 
             "+", 
             RowBox[{"606150", " ", 
              RowBox[{"cos", "(", 
               RowBox[{
                RowBox[{
                 SubscriptBox["\[Theta]", "2"], "(", "t", ")"}], "+", 
                RowBox[{
                 SubscriptBox["\[Theta]", "3"], "(", "t", ")"}], "-", 
                RowBox[{
                 SubscriptBox["\[Theta]", "4"], "(", "t", ")"}]}], ")"}]}], 
             "-", 
             RowBox[{"25096253337", " ", 
              RowBox[{"sin", "(", 
               RowBox[{
                SubscriptBox["\[Theta]", "2"], "(", "t", ")"}], ")"}]}], "-", 
             
             RowBox[{"401207755", " ", 
              RowBox[{"sin", "(", 
               RowBox[{
                RowBox[{
                 SubscriptBox["\[Theta]", "2"], "(", "t", ")"}], "+", 
                RowBox[{
                 SubscriptBox["\[Theta]", "3"], "(", "t", ")"}]}], ")"}]}], 
             "+", 
             RowBox[{"6820902539", " ", 
              RowBox[{"sin", "(", 
               RowBox[{
                RowBox[{
                 SubscriptBox["\[Theta]", "2"], "(", "t", ")"}], "+", 
                RowBox[{
                 SubscriptBox["\[Theta]", "3"], "(", "t", ")"}], "-", 
                RowBox[{
                 SubscriptBox["\[Theta]", "4"], "(", "t", ")"}]}], ")"}]}], 
             "+", 
             RowBox[{"701338095", " ", 
              RowBox[{"sin", "(", 
               RowBox[{
                RowBox[{
                 SubscriptBox["\[Theta]", "2"], "(", "t", ")"}], "+", 
                RowBox[{
                 SubscriptBox["\[Theta]", "3"], "(", "t", ")"}], "-", 
                RowBox[{
                 SubscriptBox["\[Theta]", "4"], "(", "t", ")"}], "-", 
                RowBox[{
                 SubscriptBox["\[Theta]", "5"], "(", "t", ")"}]}], ")"}]}]}], 
            ")"}], " ", 
           RowBox[{
            SuperscriptBox[
             SubscriptBox["\[Theta]", "2"], "\[Prime]",
             MultilineFunction->None], "(", "t", ")"}]}], "+", 
          RowBox[{
           RowBox[{"(", 
            RowBox[{
             RowBox[{
              RowBox[{"-", "4964191440"}], " ", 
              RowBox[{"cos", "(", 
               RowBox[{
                SubscriptBox["\[Theta]", "3"], "(", "t", ")"}], ")"}]}], "-", 
             
             RowBox[{"6615702600", " ", 
              RowBox[{"cos", "(", 
               RowBox[{
                RowBox[{
                 SubscriptBox["\[Theta]", "2"], "(", "t", ")"}], "+", 
                RowBox[{
                 SubscriptBox["\[Theta]", "3"], "(", "t", ")"}]}], ")"}]}], 
             "+", 
             RowBox[{"311561100", " ", 
              RowBox[{"cos", "(", 
               RowBox[{
                RowBox[{
                 SubscriptBox["\[Theta]", "3"], "(", "t", ")"}], "-", 
                RowBox[{
                 SubscriptBox["\[Theta]", "4"], "(", "t", ")"}]}], ")"}]}], 
             "+", 
             RowBox[{"415212750", " ", 
              RowBox[{"cos", "(", 
               RowBox[{
                RowBox[{
                 SubscriptBox["\[Theta]", "2"], "(", "t", ")"}], "+", 
                RowBox[{
                 SubscriptBox["\[Theta]", "3"], "(", "t", ")"}], "-", 
                RowBox[{
                 SubscriptBox["\[Theta]", "4"], "(", "t", ")"}]}], ")"}]}], 
             "-", 
             RowBox[{"206220786070", " ", 
              RowBox[{"sin", "(", 
               RowBox[{
                SubscriptBox["\[Theta]", "3"], "(", "t", ")"}], ")"}]}], "-", 
             
             RowBox[{"274827312175", " ", 
              RowBox[{"sin", "(", 
               RowBox[{
                RowBox[{
                 SubscriptBox["\[Theta]", "2"], "(", "t", ")"}], "+", 
                RowBox[{
                 SubscriptBox["\[Theta]", "3"], "(", "t", ")"}]}], ")"}]}], 
             "+", 
             RowBox[{"3505943905046", " ", 
              RowBox[{"sin", "(", 
               RowBox[{
                RowBox[{
                 SubscriptBox["\[Theta]", "3"], "(", "t", ")"}], "-", 
                RowBox[{
                 SubscriptBox["\[Theta]", "4"], "(", "t", ")"}]}], ")"}]}], 
             "+", 
             RowBox[{"4672318239215", " ", 
              RowBox[{"sin", "(", 
               RowBox[{
                RowBox[{
                 SubscriptBox["\[Theta]", "2"], "(", "t", ")"}], "+", 
                RowBox[{
                 SubscriptBox["\[Theta]", "3"], "(", "t", ")"}], "-", 
                RowBox[{
                 SubscriptBox["\[Theta]", "4"], "(", "t", ")"}]}], ")"}]}], 
             "+", 
             RowBox[{"360487780830", " ", 
              RowBox[{"sin", "(", 
               RowBox[{
                RowBox[{
                 SubscriptBox["\[Theta]", "3"], "(", "t", ")"}], "-", 
                RowBox[{
                 SubscriptBox["\[Theta]", "4"], "(", "t", ")"}], "-", 
                RowBox[{
                 SubscriptBox["\[Theta]", "5"], "(", "t", ")"}]}], ")"}]}], 
             "+", 
             RowBox[{"480416595075", " ", 
              RowBox[{"sin", "(", 
               RowBox[{
                RowBox[{
                 SubscriptBox["\[Theta]", "2"], "(", "t", ")"}], "+", 
                RowBox[{
                 SubscriptBox["\[Theta]", "3"], "(", "t", ")"}], "-", 
                RowBox[{
                 SubscriptBox["\[Theta]", "4"], "(", "t", ")"}], "-", 
                RowBox[{
                 SubscriptBox["\[Theta]", "5"], "(", "t", ")"}]}], ")"}]}]}], 
            ")"}], " ", 
           RowBox[{
            SuperscriptBox[
             SubscriptBox["\[Theta]", "3"], "\[Prime]",
             MultilineFunction->None], "(", "t", ")"}]}], "-", 
          RowBox[{"311561100", " ", 
           RowBox[{"cos", "(", 
            RowBox[{
             RowBox[{
              SubscriptBox["\[Theta]", "3"], "(", "t", ")"}], "-", 
             RowBox[{
              SubscriptBox["\[Theta]", "4"], "(", "t", ")"}]}], ")"}], " ", 
           RowBox[{
            SuperscriptBox[
             SubscriptBox["\[Theta]", "4"], "\[Prime]",
             MultilineFunction->None], "(", "t", ")"}]}], "-", 
          RowBox[{"415212750", " ", 
           RowBox[{"cos", "(", 
            RowBox[{
             RowBox[{
              SubscriptBox["\[Theta]", "2"], "(", "t", ")"}], "+", 
             RowBox[{
              SubscriptBox["\[Theta]", "3"], "(", "t", ")"}], "-", 
             RowBox[{
              SubscriptBox["\[Theta]", "4"], "(", "t", ")"}]}], ")"}], " ", 
           RowBox[{
            SuperscriptBox[
             SubscriptBox["\[Theta]", "4"], "\[Prime]",
             MultilineFunction->None], "(", "t", ")"}]}], "-", 
          RowBox[{"3505943905046", " ", 
           RowBox[{"sin", "(", 
            RowBox[{
             RowBox[{
              SubscriptBox["\[Theta]", "3"], "(", "t", ")"}], "-", 
             RowBox[{
              SubscriptBox["\[Theta]", "4"], "(", "t", ")"}]}], ")"}], " ", 
           RowBox[{
            SuperscriptBox[
             SubscriptBox["\[Theta]", "4"], "\[Prime]",
             MultilineFunction->None], "(", "t", ")"}]}], "-", 
          RowBox[{"4672318239215", " ", 
           RowBox[{"sin", "(", 
            RowBox[{
             RowBox[{
              SubscriptBox["\[Theta]", "2"], "(", "t", ")"}], "+", 
             RowBox[{
              SubscriptBox["\[Theta]", "3"], "(", "t", ")"}], "-", 
             RowBox[{
              SubscriptBox["\[Theta]", "4"], "(", "t", ")"}]}], ")"}], " ", 
           RowBox[{
            SuperscriptBox[
             SubscriptBox["\[Theta]", "4"], "\[Prime]",
             MultilineFunction->None], "(", "t", ")"}]}], "-", 
          RowBox[{"360487780830", " ", 
           RowBox[{"sin", "(", 
            RowBox[{
             RowBox[{
              SubscriptBox["\[Theta]", "3"], "(", "t", ")"}], "-", 
             RowBox[{
              SubscriptBox["\[Theta]", "4"], "(", "t", ")"}], "-", 
             RowBox[{
              SubscriptBox["\[Theta]", "5"], "(", "t", ")"}]}], ")"}], " ", 
           RowBox[{
            SuperscriptBox[
             SubscriptBox["\[Theta]", "4"], "\[Prime]",
             MultilineFunction->None], "(", "t", ")"}]}], "-", 
          RowBox[{"480416595075", " ", 
           RowBox[{"sin", "(", 
            RowBox[{
             RowBox[{
              SubscriptBox["\[Theta]", "2"], "(", "t", ")"}], "+", 
             RowBox[{
              SubscriptBox["\[Theta]", "3"], "(", "t", ")"}], "-", 
             RowBox[{
              SubscriptBox["\[Theta]", "4"], "(", "t", ")"}], "-", 
             RowBox[{
              SubscriptBox["\[Theta]", "5"], "(", "t", ")"}]}], ")"}], " ", 
           RowBox[{
            SuperscriptBox[
             SubscriptBox["\[Theta]", "4"], "\[Prime]",
             MultilineFunction->None], "(", "t", ")"}]}], "-", 
          RowBox[{"360487780830", " ", 
           RowBox[{"sin", "(", 
            RowBox[{
             RowBox[{
              SubscriptBox["\[Theta]", "3"], "(", "t", ")"}], "-", 
             RowBox[{
              SubscriptBox["\[Theta]", "4"], "(", "t", ")"}], "-", 
             RowBox[{
              SubscriptBox["\[Theta]", "5"], "(", "t", ")"}]}], ")"}], " ", 
           RowBox[{
            SuperscriptBox[
             SubscriptBox["\[Theta]", "5"], "\[Prime]",
             MultilineFunction->None], "(", "t", ")"}]}], "-", 
          RowBox[{"480416595075", " ", 
           RowBox[{"sin", "(", 
            RowBox[{
             RowBox[{
              SubscriptBox["\[Theta]", "2"], "(", "t", ")"}], "+", 
             RowBox[{
              SubscriptBox["\[Theta]", "3"], "(", "t", ")"}], "-", 
             RowBox[{
              SubscriptBox["\[Theta]", "4"], "(", "t", ")"}], "-", 
             RowBox[{
              SubscriptBox["\[Theta]", "5"], "(", "t", ")"}]}], ")"}], " ", 
           RowBox[{
            SuperscriptBox[
             SubscriptBox["\[Theta]", "5"], "\[Prime]",
             MultilineFunction->None], "(", "t", ")"}]}], "-", 
          RowBox[{"360487780830", " ", 
           RowBox[{"sin", "(", 
            RowBox[{
             SubscriptBox["\[Theta]", "5"], "(", "t", ")"}], ")"}], " ", 
           RowBox[{
            SuperscriptBox[
             SubscriptBox["\[Theta]", "5"], "\[Prime]",
             MultilineFunction->None], "(", "t", ")"}]}]}], ")"}]}], 
       "12500000000000000"], 
      FractionBox[
       RowBox[{"127", " ", 
        RowBox[{"(", 
         RowBox[{
          RowBox[{"685", " ", 
           RowBox[{"(", 
            RowBox[{
             RowBox[{"606150", " ", 
              RowBox[{"cos", "(", 
               RowBox[{
                SubscriptBox["\[Theta]", "2"], "(", "t", ")"}], ")"}]}], "-", 
             
             RowBox[{"9657960", " ", 
              RowBox[{"cos", "(", 
               RowBox[{
                RowBox[{
                 SubscriptBox["\[Theta]", "2"], "(", "t", ")"}], "+", 
                RowBox[{
                 SubscriptBox["\[Theta]", "3"], "(", "t", ")"}]}], ")"}]}], 
             "+", 
             RowBox[{"606150", " ", 
              RowBox[{"cos", "(", 
               RowBox[{
                RowBox[{
                 SubscriptBox["\[Theta]", "2"], "(", "t", ")"}], "+", 
                RowBox[{
                 SubscriptBox["\[Theta]", "3"], "(", "t", ")"}], "-", 
                RowBox[{
                 SubscriptBox["\[Theta]", "4"], "(", "t", ")"}]}], ")"}]}], 
             "-", 
             RowBox[{"25096253337", " ", 
              RowBox[{"sin", "(", 
               RowBox[{
                SubscriptBox["\[Theta]", "2"], "(", "t", ")"}], ")"}]}], "-", 
             
             RowBox[{"401207755", " ", 
              RowBox[{"sin", "(", 
               RowBox[{
                RowBox[{
                 SubscriptBox["\[Theta]", "2"], "(", "t", ")"}], "+", 
                RowBox[{
                 SubscriptBox["\[Theta]", "3"], "(", "t", ")"}]}], ")"}]}], 
             "+", 
             RowBox[{"6820902539", " ", 
              RowBox[{"sin", "(", 
               RowBox[{
                RowBox[{
                 SubscriptBox["\[Theta]", "2"], "(", "t", ")"}], "+", 
                RowBox[{
                 SubscriptBox["\[Theta]", "3"], "(", "t", ")"}], "-", 
                RowBox[{
                 SubscriptBox["\[Theta]", "4"], "(", "t", ")"}]}], ")"}]}], 
             "+", 
             RowBox[{"701338095", " ", 
              RowBox[{"sin", "(", 
               RowBox[{
                RowBox[{
                 SubscriptBox["\[Theta]", "2"], "(", "t", ")"}], "+", 
                RowBox[{
                 SubscriptBox["\[Theta]", "3"], "(", "t", ")"}], "-", 
                RowBox[{
                 SubscriptBox["\[Theta]", "4"], "(", "t", ")"}], "-", 
                RowBox[{
                 SubscriptBox["\[Theta]", "5"], "(", "t", ")"}]}], ")"}]}]}], 
            ")"}], " ", 
           RowBox[{
            SuperscriptBox[
             SubscriptBox["\[Theta]", "1"], "\[Prime]",
             MultilineFunction->None], "(", "t", ")"}]}], "+", 
          RowBox[{"685", " ", 
           RowBox[{"(", 
            RowBox[{
             RowBox[{"606150", " ", 
              RowBox[{"cos", "(", 
               RowBox[{
                SubscriptBox["\[Theta]", "2"], "(", "t", ")"}], ")"}]}], "-", 
             
             RowBox[{"9657960", " ", 
              RowBox[{"cos", "(", 
               RowBox[{
                RowBox[{
                 SubscriptBox["\[Theta]", "2"], "(", "t", ")"}], "+", 
                RowBox[{
                 SubscriptBox["\[Theta]", "3"], "(", "t", ")"}]}], ")"}]}], 
             "+", 
             RowBox[{"606150", " ", 
              RowBox[{"cos", "(", 
               RowBox[{
                RowBox[{
                 SubscriptBox["\[Theta]", "2"], "(", "t", ")"}], "+", 
                RowBox[{
                 SubscriptBox["\[Theta]", "3"], "(", "t", ")"}], "-", 
                RowBox[{
                 SubscriptBox["\[Theta]", "4"], "(", "t", ")"}]}], ")"}]}], 
             "-", 
             RowBox[{"25096253337", " ", 
              RowBox[{"sin", "(", 
               RowBox[{
                SubscriptBox["\[Theta]", "2"], "(", "t", ")"}], ")"}]}], "-", 
             
             RowBox[{"401207755", " ", 
              RowBox[{"sin", "(", 
               RowBox[{
                RowBox[{
                 SubscriptBox["\[Theta]", "2"], "(", "t", ")"}], "+", 
                RowBox[{
                 SubscriptBox["\[Theta]", "3"], "(", "t", ")"}]}], ")"}]}], 
             "+", 
             RowBox[{"6820902539", " ", 
              RowBox[{"sin", "(", 
               RowBox[{
                RowBox[{
                 SubscriptBox["\[Theta]", "2"], "(", "t", ")"}], "+", 
                RowBox[{
                 SubscriptBox["\[Theta]", "3"], "(", "t", ")"}], "-", 
                RowBox[{
                 SubscriptBox["\[Theta]", "4"], "(", "t", ")"}]}], ")"}]}], 
             "+", 
             RowBox[{"701338095", " ", 
              RowBox[{"sin", "(", 
               RowBox[{
                RowBox[{
                 SubscriptBox["\[Theta]", "2"], "(", "t", ")"}], "+", 
                RowBox[{
                 SubscriptBox["\[Theta]", "3"], "(", "t", ")"}], "-", 
                RowBox[{
                 SubscriptBox["\[Theta]", "4"], "(", "t", ")"}], "-", 
                RowBox[{
                 SubscriptBox["\[Theta]", "5"], "(", "t", ")"}]}], ")"}]}]}], 
            ")"}], " ", 
           RowBox[{
            SuperscriptBox[
             SubscriptBox["\[Theta]", "2"], "\[Prime]",
             MultilineFunction->None], "(", "t", ")"}]}], "-", 
          RowBox[{"4964191440", " ", 
           RowBox[{"cos", "(", 
            RowBox[{
             SubscriptBox["\[Theta]", "3"], "(", "t", ")"}], ")"}], " ", 
           RowBox[{
            SuperscriptBox[
             SubscriptBox["\[Theta]", "3"], "\[Prime]",
             MultilineFunction->None], "(", "t", ")"}]}], "-", 
          RowBox[{"6615702600", " ", 
           RowBox[{"cos", "(", 
            RowBox[{
             RowBox[{
              SubscriptBox["\[Theta]", "2"], "(", "t", ")"}], "+", 
             RowBox[{
              SubscriptBox["\[Theta]", "3"], "(", "t", ")"}]}], ")"}], " ", 
           RowBox[{
            SuperscriptBox[
             SubscriptBox["\[Theta]", "3"], "\[Prime]",
             MultilineFunction->None], "(", "t", ")"}]}], "+", 
          RowBox[{"311561100", " ", 
           RowBox[{"cos", "(", 
            RowBox[{
             RowBox[{
              SubscriptBox["\[Theta]", "3"], "(", "t", ")"}], "-", 
             RowBox[{
              SubscriptBox["\[Theta]", "4"], "(", "t", ")"}]}], ")"}], " ", 
           RowBox[{
            SuperscriptBox[
             SubscriptBox["\[Theta]", "3"], "\[Prime]",
             MultilineFunction->None], "(", "t", ")"}]}], "+", 
          RowBox[{"415212750", " ", 
           RowBox[{"cos", "(", 
            RowBox[{
             RowBox[{
              SubscriptBox["\[Theta]", "2"], "(", "t", ")"}], "+", 
             RowBox[{
              SubscriptBox["\[Theta]", "3"], "(", "t", ")"}], "-", 
             RowBox[{
              SubscriptBox["\[Theta]", "4"], "(", "t", ")"}]}], ")"}], " ", 
           RowBox[{
            SuperscriptBox[
             SubscriptBox["\[Theta]", "3"], "\[Prime]",
             MultilineFunction->None], "(", "t", ")"}]}], "-", 
          RowBox[{"206220786070", " ", 
           RowBox[{"sin", "(", 
            RowBox[{
             SubscriptBox["\[Theta]", "3"], "(", "t", ")"}], ")"}], " ", 
           RowBox[{
            SuperscriptBox[
             SubscriptBox["\[Theta]", "3"], "\[Prime]",
             MultilineFunction->None], "(", "t", ")"}]}], "-", 
          RowBox[{"274827312175", " ", 
           RowBox[{"sin", "(", 
            RowBox[{
             RowBox[{
              SubscriptBox["\[Theta]", "2"], "(", "t", ")"}], "+", 
             RowBox[{
              SubscriptBox["\[Theta]", "3"], "(", "t", ")"}]}], ")"}], " ", 
           RowBox[{
            SuperscriptBox[
             SubscriptBox["\[Theta]", "3"], "\[Prime]",
             MultilineFunction->None], "(", "t", ")"}]}], "+", 
          RowBox[{"3505943905046", " ", 
           RowBox[{"sin", "(", 
            RowBox[{
             RowBox[{
              SubscriptBox["\[Theta]", "3"], "(", "t", ")"}], "-", 
             RowBox[{
              SubscriptBox["\[Theta]", "4"], "(", "t", ")"}]}], ")"}], " ", 
           RowBox[{
            SuperscriptBox[
             SubscriptBox["\[Theta]", "3"], "\[Prime]",
             MultilineFunction->None], "(", "t", ")"}]}], "+", 
          RowBox[{"4672318239215", " ", 
           RowBox[{"sin", "(", 
            RowBox[{
             RowBox[{
              SubscriptBox["\[Theta]", "2"], "(", "t", ")"}], "+", 
             RowBox[{
              SubscriptBox["\[Theta]", "3"], "(", "t", ")"}], "-", 
             RowBox[{
              SubscriptBox["\[Theta]", "4"], "(", "t", ")"}]}], ")"}], " ", 
           RowBox[{
            SuperscriptBox[
             SubscriptBox["\[Theta]", "3"], "\[Prime]",
             MultilineFunction->None], "(", "t", ")"}]}], "+", 
          RowBox[{"360487780830", " ", 
           RowBox[{"sin", "(", 
            RowBox[{
             RowBox[{
              SubscriptBox["\[Theta]", "3"], "(", "t", ")"}], "-", 
             RowBox[{
              SubscriptBox["\[Theta]", "4"], "(", "t", ")"}], "-", 
             RowBox[{
              SubscriptBox["\[Theta]", "5"], "(", "t", ")"}]}], ")"}], " ", 
           RowBox[{
            SuperscriptBox[
             SubscriptBox["\[Theta]", "3"], "\[Prime]",
             MultilineFunction->None], "(", "t", ")"}]}], "+", 
          RowBox[{"480416595075", " ", 
           RowBox[{"sin", "(", 
            RowBox[{
             RowBox[{
              SubscriptBox["\[Theta]", "2"], "(", "t", ")"}], "+", 
             RowBox[{
              SubscriptBox["\[Theta]", "3"], "(", "t", ")"}], "-", 
             RowBox[{
              SubscriptBox["\[Theta]", "4"], "(", "t", ")"}], "-", 
             RowBox[{
              SubscriptBox["\[Theta]", "5"], "(", "t", ")"}]}], ")"}], " ", 
           RowBox[{
            SuperscriptBox[
             SubscriptBox["\[Theta]", "3"], "\[Prime]",
             MultilineFunction->None], "(", "t", ")"}]}], "-", 
          RowBox[{"311561100", " ", 
           RowBox[{"cos", "(", 
            RowBox[{
             RowBox[{
              SubscriptBox["\[Theta]", "3"], "(", "t", ")"}], "-", 
             RowBox[{
              SubscriptBox["\[Theta]", "4"], "(", "t", ")"}]}], ")"}], " ", 
           RowBox[{
            SuperscriptBox[
             SubscriptBox["\[Theta]", "4"], "\[Prime]",
             MultilineFunction->None], "(", "t", ")"}]}], "-", 
          RowBox[{"415212750", " ", 
           RowBox[{"cos", "(", 
            RowBox[{
             RowBox[{
              SubscriptBox["\[Theta]", "2"], "(", "t", ")"}], "+", 
             RowBox[{
              SubscriptBox["\[Theta]", "3"], "(", "t", ")"}], "-", 
             RowBox[{
              SubscriptBox["\[Theta]", "4"], "(", "t", ")"}]}], ")"}], " ", 
           RowBox[{
            SuperscriptBox[
             SubscriptBox["\[Theta]", "4"], "\[Prime]",
             MultilineFunction->None], "(", "t", ")"}]}], "-", 
          RowBox[{"3505943905046", " ", 
           RowBox[{"sin", "(", 
            RowBox[{
             RowBox[{
              SubscriptBox["\[Theta]", "3"], "(", "t", ")"}], "-", 
             RowBox[{
              SubscriptBox["\[Theta]", "4"], "(", "t", ")"}]}], ")"}], " ", 
           RowBox[{
            SuperscriptBox[
             SubscriptBox["\[Theta]", "4"], "\[Prime]",
             MultilineFunction->None], "(", "t", ")"}]}], "-", 
          RowBox[{"4672318239215", " ", 
           RowBox[{"sin", "(", 
            RowBox[{
             RowBox[{
              SubscriptBox["\[Theta]", "2"], "(", "t", ")"}], "+", 
             RowBox[{
              SubscriptBox["\[Theta]", "3"], "(", "t", ")"}], "-", 
             RowBox[{
              SubscriptBox["\[Theta]", "4"], "(", "t", ")"}]}], ")"}], " ", 
           RowBox[{
            SuperscriptBox[
             SubscriptBox["\[Theta]", "4"], "\[Prime]",
             MultilineFunction->None], "(", "t", ")"}]}], "-", 
          RowBox[{"360487780830", " ", 
           RowBox[{"sin", "(", 
            RowBox[{
             RowBox[{
              SubscriptBox["\[Theta]", "3"], "(", "t", ")"}], "-", 
             RowBox[{
              SubscriptBox["\[Theta]", "4"], "(", "t", ")"}], "-", 
             RowBox[{
              SubscriptBox["\[Theta]", "5"], "(", "t", ")"}]}], ")"}], " ", 
           RowBox[{
            SuperscriptBox[
             SubscriptBox["\[Theta]", "4"], "\[Prime]",
             MultilineFunction->None], "(", "t", ")"}]}], "-", 
          RowBox[{"480416595075", " ", 
           RowBox[{"sin", "(", 
            RowBox[{
             RowBox[{
              SubscriptBox["\[Theta]", "2"], "(", "t", ")"}], "+", 
             RowBox[{
              SubscriptBox["\[Theta]", "3"], "(", "t", ")"}], "-", 
             RowBox[{
              SubscriptBox["\[Theta]", "4"], "(", "t", ")"}], "-", 
             RowBox[{
              SubscriptBox["\[Theta]", "5"], "(", "t", ")"}]}], ")"}], " ", 
           RowBox[{
            SuperscriptBox[
             SubscriptBox["\[Theta]", "4"], "\[Prime]",
             MultilineFunction->None], "(", "t", ")"}]}], "-", 
          RowBox[{"360487780830", " ", 
           RowBox[{"sin", "(", 
            RowBox[{
             RowBox[{
              SubscriptBox["\[Theta]", "3"], "(", "t", ")"}], "-", 
             RowBox[{
              SubscriptBox["\[Theta]", "4"], "(", "t", ")"}], "-", 
             RowBox[{
              SubscriptBox["\[Theta]", "5"], "(", "t", ")"}]}], ")"}], " ", 
           RowBox[{
            SuperscriptBox[
             SubscriptBox["\[Theta]", "5"], "\[Prime]",
             MultilineFunction->None], "(", "t", ")"}]}], "-", 
          RowBox[{"480416595075", " ", 
           RowBox[{"sin", "(", 
            RowBox[{
             RowBox[{
              SubscriptBox["\[Theta]", "2"], "(", "t", ")"}], "+", 
             RowBox[{
              SubscriptBox["\[Theta]", "3"], "(", "t", ")"}], "-", 
             RowBox[{
              SubscriptBox["\[Theta]", "4"], "(", "t", ")"}], "-", 
             RowBox[{
              SubscriptBox["\[Theta]", "5"], "(", "t", ")"}]}], ")"}], " ", 
           RowBox[{
            SuperscriptBox[
             SubscriptBox["\[Theta]", "5"], "\[Prime]",
             MultilineFunction->None], "(", "t", ")"}]}], "-", 
          RowBox[{"360487780830", " ", 
           RowBox[{"sin", "(", 
            RowBox[{
             SubscriptBox["\[Theta]", "5"], "(", "t", ")"}], ")"}], " ", 
           RowBox[{
            SuperscriptBox[
             SubscriptBox["\[Theta]", "5"], "\[Prime]",
             MultilineFunction->None], "(", "t", ")"}]}]}], ")"}]}], 
       "12500000000000000"], 
      RowBox[{"-", 
       FractionBox[
        RowBox[{"127", " ", 
         RowBox[{"(", 
          RowBox[{
           RowBox[{
            RowBox[{"-", 
             RowBox[{"(", 
              RowBox[{
               RowBox[{
                RowBox[{"-", "4964191440"}], " ", 
                RowBox[{"cos", "(", 
                 RowBox[{
                  SubscriptBox["\[Theta]", "3"], "(", "t", ")"}], ")"}]}], 
               "-", 
               RowBox[{"6615702600", " ", 
                RowBox[{"cos", "(", 
                 RowBox[{
                  RowBox[{
                   SubscriptBox["\[Theta]", "2"], "(", "t", ")"}], "+", 
                  RowBox[{
                   SubscriptBox["\[Theta]", "3"], "(", "t", ")"}]}], ")"}]}], 
               "+", 
               RowBox[{"311561100", " ", 
                RowBox[{"cos", "(", 
                 RowBox[{
                  RowBox[{
                   SubscriptBox["\[Theta]", "3"], "(", "t", ")"}], "-", 
                  RowBox[{
                   SubscriptBox["\[Theta]", "4"], "(", "t", ")"}]}], ")"}]}], 
               "+", 
               RowBox[{"415212750", " ", 
                RowBox[{"cos", "(", 
                 RowBox[{
                  RowBox[{
                   SubscriptBox["\[Theta]", "2"], "(", "t", ")"}], "+", 
                  RowBox[{
                   SubscriptBox["\[Theta]", "3"], "(", "t", ")"}], "-", 
                  RowBox[{
                   SubscriptBox["\[Theta]", "4"], "(", "t", ")"}]}], ")"}]}], 
               "-", 
               RowBox[{"206220786070", " ", 
                RowBox[{"sin", "(", 
                 RowBox[{
                  SubscriptBox["\[Theta]", "3"], "(", "t", ")"}], ")"}]}], 
               "-", 
               RowBox[{"274827312175", " ", 
                RowBox[{"sin", "(", 
                 RowBox[{
                  RowBox[{
                   SubscriptBox["\[Theta]", "2"], "(", "t", ")"}], "+", 
                  RowBox[{
                   SubscriptBox["\[Theta]", "3"], "(", "t", ")"}]}], ")"}]}], 
               "+", 
               RowBox[{"3505943905046", " ", 
                RowBox[{"sin", "(", 
                 RowBox[{
                  RowBox[{
                   SubscriptBox["\[Theta]", "3"], "(", "t", ")"}], "-", 
                  RowBox[{
                   SubscriptBox["\[Theta]", "4"], "(", "t", ")"}]}], ")"}]}], 
               "+", 
               RowBox[{"4672318239215", " ", 
                RowBox[{"sin", "(", 
                 RowBox[{
                  RowBox[{
                   SubscriptBox["\[Theta]", "2"], "(", "t", ")"}], "+", 
                  RowBox[{
                   SubscriptBox["\[Theta]", "3"], "(", "t", ")"}], "-", 
                  RowBox[{
                   SubscriptBox["\[Theta]", "4"], "(", "t", ")"}]}], ")"}]}], 
               "+", 
               RowBox[{"360487780830", " ", 
                RowBox[{"sin", "(", 
                 RowBox[{
                  RowBox[{
                   SubscriptBox["\[Theta]", "3"], "(", "t", ")"}], "-", 
                  RowBox[{
                   SubscriptBox["\[Theta]", "4"], "(", "t", ")"}], "-", 
                  RowBox[{
                   SubscriptBox["\[Theta]", "5"], "(", "t", ")"}]}], ")"}]}], 
               "+", 
               RowBox[{"480416595075", " ", 
                RowBox[{"sin", "(", 
                 RowBox[{
                  RowBox[{
                   SubscriptBox["\[Theta]", "2"], "(", "t", ")"}], "+", 
                  RowBox[{
                   SubscriptBox["\[Theta]", "3"], "(", "t", ")"}], "-", 
                  RowBox[{
                   SubscriptBox["\[Theta]", "4"], "(", "t", ")"}], "-", 
                  RowBox[{
                   SubscriptBox["\[Theta]", "5"], "(", "t", ")"}]}], 
                 ")"}]}]}], ")"}]}], " ", 
            RowBox[{
             SuperscriptBox[
              SubscriptBox["\[Theta]", "1"], "\[Prime]",
              MultilineFunction->None], "(", "t", ")"}]}], "-", 
           RowBox[{
            RowBox[{"(", 
             RowBox[{
              RowBox[{
               RowBox[{"-", "4964191440"}], " ", 
               RowBox[{"cos", "(", 
                RowBox[{
                 SubscriptBox["\[Theta]", "3"], "(", "t", ")"}], ")"}]}], "-", 
              RowBox[{"6615702600", " ", 
               RowBox[{"cos", "(", 
                RowBox[{
                 RowBox[{
                  SubscriptBox["\[Theta]", "2"], "(", "t", ")"}], "+", 
                 RowBox[{
                  SubscriptBox["\[Theta]", "3"], "(", "t", ")"}]}], ")"}]}], 
              "+", 
              RowBox[{"311561100", " ", 
               RowBox[{"cos", "(", 
                RowBox[{
                 RowBox[{
                  SubscriptBox["\[Theta]", "3"], "(", "t", ")"}], "-", 
                 RowBox[{
                  SubscriptBox["\[Theta]", "4"], "(", "t", ")"}]}], ")"}]}], 
              "+", 
              RowBox[{"415212750", " ", 
               RowBox[{"cos", "(", 
                RowBox[{
                 RowBox[{
                  SubscriptBox["\[Theta]", "2"], "(", "t", ")"}], "+", 
                 RowBox[{
                  SubscriptBox["\[Theta]", "3"], "(", "t", ")"}], "-", 
                 RowBox[{
                  SubscriptBox["\[Theta]", "4"], "(", "t", ")"}]}], ")"}]}], 
              "-", 
              RowBox[{"206220786070", " ", 
               RowBox[{"sin", "(", 
                RowBox[{
                 SubscriptBox["\[Theta]", "3"], "(", "t", ")"}], ")"}]}], "-", 
              RowBox[{"274827312175", " ", 
               RowBox[{"sin", "(", 
                RowBox[{
                 RowBox[{
                  SubscriptBox["\[Theta]", "2"], "(", "t", ")"}], "+", 
                 RowBox[{
                  SubscriptBox["\[Theta]", "3"], "(", "t", ")"}]}], ")"}]}], 
              "+", 
              RowBox[{"3505943905046", " ", 
               RowBox[{"sin", "(", 
                RowBox[{
                 RowBox[{
                  SubscriptBox["\[Theta]", "3"], "(", "t", ")"}], "-", 
                 RowBox[{
                  SubscriptBox["\[Theta]", "4"], "(", "t", ")"}]}], ")"}]}], 
              "+", 
              RowBox[{"4672318239215", " ", 
               RowBox[{"sin", "(", 
                RowBox[{
                 RowBox[{
                  SubscriptBox["\[Theta]", "2"], "(", "t", ")"}], "+", 
                 RowBox[{
                  SubscriptBox["\[Theta]", "3"], "(", "t", ")"}], "-", 
                 RowBox[{
                  SubscriptBox["\[Theta]", "4"], "(", "t", ")"}]}], ")"}]}], 
              "+", 
              RowBox[{"360487780830", " ", 
               RowBox[{"sin", "(", 
                RowBox[{
                 RowBox[{
                  SubscriptBox["\[Theta]", "3"], "(", "t", ")"}], "-", 
                 RowBox[{
                  SubscriptBox["\[Theta]", "4"], "(", "t", ")"}], "-", 
                 RowBox[{
                  SubscriptBox["\[Theta]", "5"], "(", "t", ")"}]}], ")"}]}], 
              "+", 
              RowBox[{"480416595075", " ", 
               RowBox[{"sin", "(", 
                RowBox[{
                 RowBox[{
                  SubscriptBox["\[Theta]", "2"], "(", "t", ")"}], "+", 
                 RowBox[{
                  SubscriptBox["\[Theta]", "3"], "(", "t", ")"}], "-", 
                 RowBox[{
                  SubscriptBox["\[Theta]", "4"], "(", "t", ")"}], "-", 
                 RowBox[{
                  SubscriptBox["\[Theta]", "5"], "(", "t", ")"}]}], ")"}]}]}],
              ")"}], " ", 
            RowBox[{
             SuperscriptBox[
              SubscriptBox["\[Theta]", "2"], "\[Prime]",
              MultilineFunction->None], "(", "t", ")"}]}], "+", 
           RowBox[{"4964191440", " ", 
            RowBox[{"cos", "(", 
             RowBox[{
              SubscriptBox["\[Theta]", "3"], "(", "t", ")"}], ")"}], " ", 
            RowBox[{
             SuperscriptBox[
              SubscriptBox["\[Theta]", "3"], "\[Prime]",
              MultilineFunction->None], "(", "t", ")"}]}], "+", 
           RowBox[{"6615702600", " ", 
            RowBox[{"cos", "(", 
             RowBox[{
              RowBox[{
               SubscriptBox["\[Theta]", "2"], "(", "t", ")"}], "+", 
              RowBox[{
               SubscriptBox["\[Theta]", "3"], "(", "t", ")"}]}], ")"}], " ", 
            RowBox[{
             SuperscriptBox[
              SubscriptBox["\[Theta]", "3"], "\[Prime]",
              MultilineFunction->None], "(", "t", ")"}]}], "-", 
           RowBox[{"311561100", " ", 
            RowBox[{"cos", "(", 
             RowBox[{
              RowBox[{
               SubscriptBox["\[Theta]", "3"], "(", "t", ")"}], "-", 
              RowBox[{
               SubscriptBox["\[Theta]", "4"], "(", "t", ")"}]}], ")"}], " ", 
            RowBox[{
             SuperscriptBox[
              SubscriptBox["\[Theta]", "3"], "\[Prime]",
              MultilineFunction->None], "(", "t", ")"}]}], "-", 
           RowBox[{"415212750", " ", 
            RowBox[{"cos", "(", 
             RowBox[{
              RowBox[{
               SubscriptBox["\[Theta]", "2"], "(", "t", ")"}], "+", 
              RowBox[{
               SubscriptBox["\[Theta]", "3"], "(", "t", ")"}], "-", 
              RowBox[{
               SubscriptBox["\[Theta]", "4"], "(", "t", ")"}]}], ")"}], " ", 
            RowBox[{
             SuperscriptBox[
              SubscriptBox["\[Theta]", "3"], "\[Prime]",
              MultilineFunction->None], "(", "t", ")"}]}], "+", 
           RowBox[{"206220786070", " ", 
            RowBox[{"sin", "(", 
             RowBox[{
              SubscriptBox["\[Theta]", "3"], "(", "t", ")"}], ")"}], " ", 
            RowBox[{
             SuperscriptBox[
              SubscriptBox["\[Theta]", "3"], "\[Prime]",
              MultilineFunction->None], "(", "t", ")"}]}], "+", 
           RowBox[{"274827312175", " ", 
            RowBox[{"sin", "(", 
             RowBox[{
              RowBox[{
               SubscriptBox["\[Theta]", "2"], "(", "t", ")"}], "+", 
              RowBox[{
               SubscriptBox["\[Theta]", "3"], "(", "t", ")"}]}], ")"}], " ", 
            RowBox[{
             SuperscriptBox[
              SubscriptBox["\[Theta]", "3"], "\[Prime]",
              MultilineFunction->None], "(", "t", ")"}]}], "-", 
           RowBox[{"3505943905046", " ", 
            RowBox[{"sin", "(", 
             RowBox[{
              RowBox[{
               SubscriptBox["\[Theta]", "3"], "(", "t", ")"}], "-", 
              RowBox[{
               SubscriptBox["\[Theta]", "4"], "(", "t", ")"}]}], ")"}], " ", 
            RowBox[{
             SuperscriptBox[
              SubscriptBox["\[Theta]", "3"], "\[Prime]",
              MultilineFunction->None], "(", "t", ")"}]}], "-", 
           RowBox[{"4672318239215", " ", 
            RowBox[{"sin", "(", 
             RowBox[{
              RowBox[{
               SubscriptBox["\[Theta]", "2"], "(", "t", ")"}], "+", 
              RowBox[{
               SubscriptBox["\[Theta]", "3"], "(", "t", ")"}], "-", 
              RowBox[{
               SubscriptBox["\[Theta]", "4"], "(", "t", ")"}]}], ")"}], " ", 
            RowBox[{
             SuperscriptBox[
              SubscriptBox["\[Theta]", "3"], "\[Prime]",
              MultilineFunction->None], "(", "t", ")"}]}], "-", 
           RowBox[{"360487780830", " ", 
            RowBox[{"sin", "(", 
             RowBox[{
              RowBox[{
               SubscriptBox["\[Theta]", "3"], "(", "t", ")"}], "-", 
              RowBox[{
               SubscriptBox["\[Theta]", "4"], "(", "t", ")"}], "-", 
              RowBox[{
               SubscriptBox["\[Theta]", "5"], "(", "t", ")"}]}], ")"}], " ", 
            RowBox[{
             SuperscriptBox[
              SubscriptBox["\[Theta]", "3"], "\[Prime]",
              MultilineFunction->None], "(", "t", ")"}]}], "-", 
           RowBox[{"480416595075", " ", 
            RowBox[{"sin", "(", 
             RowBox[{
              RowBox[{
               SubscriptBox["\[Theta]", "2"], "(", "t", ")"}], "+", 
              RowBox[{
               SubscriptBox["\[Theta]", "3"], "(", "t", ")"}], "-", 
              RowBox[{
               SubscriptBox["\[Theta]", "4"], "(", "t", ")"}], "-", 
              RowBox[{
               SubscriptBox["\[Theta]", "5"], "(", "t", ")"}]}], ")"}], " ", 
            RowBox[{
             SuperscriptBox[
              SubscriptBox["\[Theta]", "3"], "\[Prime]",
              MultilineFunction->None], "(", "t", ")"}]}], "+", 
           RowBox[{"311561100", " ", 
            RowBox[{"cos", "(", 
             RowBox[{
              RowBox[{
               SubscriptBox["\[Theta]", "3"], "(", "t", ")"}], "-", 
              RowBox[{
               SubscriptBox["\[Theta]", "4"], "(", "t", ")"}]}], ")"}], " ", 
            RowBox[{
             SuperscriptBox[
              SubscriptBox["\[Theta]", "4"], "\[Prime]",
              MultilineFunction->None], "(", "t", ")"}]}], "+", 
           RowBox[{"415212750", " ", 
            RowBox[{"cos", "(", 
             RowBox[{
              RowBox[{
               SubscriptBox["\[Theta]", "2"], "(", "t", ")"}], "+", 
              RowBox[{
               SubscriptBox["\[Theta]", "3"], "(", "t", ")"}], "-", 
              RowBox[{
               SubscriptBox["\[Theta]", "4"], "(", "t", ")"}]}], ")"}], " ", 
            RowBox[{
             SuperscriptBox[
              SubscriptBox["\[Theta]", "4"], "\[Prime]",
              MultilineFunction->None], "(", "t", ")"}]}], "+", 
           RowBox[{"3505943905046", " ", 
            RowBox[{"sin", "(", 
             RowBox[{
              RowBox[{
               SubscriptBox["\[Theta]", "3"], "(", "t", ")"}], "-", 
              RowBox[{
               SubscriptBox["\[Theta]", "4"], "(", "t", ")"}]}], ")"}], " ", 
            RowBox[{
             SuperscriptBox[
              SubscriptBox["\[Theta]", "4"], "\[Prime]",
              MultilineFunction->None], "(", "t", ")"}]}], "+", 
           RowBox[{"4672318239215", " ", 
            RowBox[{"sin", "(", 
             RowBox[{
              RowBox[{
               SubscriptBox["\[Theta]", "2"], "(", "t", ")"}], "+", 
              RowBox[{
               SubscriptBox["\[Theta]", "3"], "(", "t", ")"}], "-", 
              RowBox[{
               SubscriptBox["\[Theta]", "4"], "(", "t", ")"}]}], ")"}], " ", 
            RowBox[{
             SuperscriptBox[
              SubscriptBox["\[Theta]", "4"], "\[Prime]",
              MultilineFunction->None], "(", "t", ")"}]}], "+", 
           RowBox[{"360487780830", " ", 
            RowBox[{"sin", "(", 
             RowBox[{
              RowBox[{
               SubscriptBox["\[Theta]", "3"], "(", "t", ")"}], "-", 
              RowBox[{
               SubscriptBox["\[Theta]", "4"], "(", "t", ")"}], "-", 
              RowBox[{
               SubscriptBox["\[Theta]", "5"], "(", "t", ")"}]}], ")"}], " ", 
            RowBox[{
             SuperscriptBox[
              SubscriptBox["\[Theta]", "4"], "\[Prime]",
              MultilineFunction->None], "(", "t", ")"}]}], "+", 
           RowBox[{"480416595075", " ", 
            RowBox[{"sin", "(", 
             RowBox[{
              RowBox[{
               SubscriptBox["\[Theta]", "2"], "(", "t", ")"}], "+", 
              RowBox[{
               SubscriptBox["\[Theta]", "3"], "(", "t", ")"}], "-", 
              RowBox[{
               SubscriptBox["\[Theta]", "4"], "(", "t", ")"}], "-", 
              RowBox[{
               SubscriptBox["\[Theta]", "5"], "(", "t", ")"}]}], ")"}], " ", 
            RowBox[{
             SuperscriptBox[
              SubscriptBox["\[Theta]", "4"], "\[Prime]",
              MultilineFunction->None], "(", "t", ")"}]}], "+", 
           RowBox[{"360487780830", " ", 
            RowBox[{"sin", "(", 
             RowBox[{
              RowBox[{
               SubscriptBox["\[Theta]", "3"], "(", "t", ")"}], "-", 
              RowBox[{
               SubscriptBox["\[Theta]", "4"], "(", "t", ")"}], "-", 
              RowBox[{
               SubscriptBox["\[Theta]", "5"], "(", "t", ")"}]}], ")"}], " ", 
            RowBox[{
             SuperscriptBox[
              SubscriptBox["\[Theta]", "5"], "\[Prime]",
              MultilineFunction->None], "(", "t", ")"}]}], "+", 
           RowBox[{"480416595075", " ", 
            RowBox[{"sin", "(", 
             RowBox[{
              RowBox[{
               SubscriptBox["\[Theta]", "2"], "(", "t", ")"}], "+", 
              RowBox[{
               SubscriptBox["\[Theta]", "3"], "(", "t", ")"}], "-", 
              RowBox[{
               SubscriptBox["\[Theta]", "4"], "(", "t", ")"}], "-", 
              RowBox[{
               SubscriptBox["\[Theta]", "5"], "(", "t", ")"}]}], ")"}], " ", 
            RowBox[{
             SuperscriptBox[
              SubscriptBox["\[Theta]", "5"], "\[Prime]",
              MultilineFunction->None], "(", "t", ")"}]}], "+", 
           RowBox[{"360487780830", " ", 
            RowBox[{"sin", "(", 
             RowBox[{
              SubscriptBox["\[Theta]", "5"], "(", "t", ")"}], ")"}], " ", 
            RowBox[{
             SuperscriptBox[
              SubscriptBox["\[Theta]", "5"], "\[Prime]",
              MultilineFunction->None], "(", "t", ")"}]}]}], ")"}]}], 
        "12500000000000000"]}], 
      RowBox[{"-", 
       FractionBox[
        RowBox[{"127", " ", 
         RowBox[{"(", 
          RowBox[{
           RowBox[{
            RowBox[{"(", 
             RowBox[{
              RowBox[{"311561100", " ", 
               RowBox[{"cos", "(", 
                RowBox[{
                 RowBox[{
                  SubscriptBox["\[Theta]", "3"], "(", "t", ")"}], "-", 
                 RowBox[{
                  SubscriptBox["\[Theta]", "4"], "(", "t", ")"}]}], ")"}]}], 
              "+", 
              RowBox[{"415212750", " ", 
               RowBox[{"cos", "(", 
                RowBox[{
                 RowBox[{
                  SubscriptBox["\[Theta]", "2"], "(", "t", ")"}], "+", 
                 RowBox[{
                  SubscriptBox["\[Theta]", "3"], "(", "t", ")"}], "-", 
                 RowBox[{
                  SubscriptBox["\[Theta]", "4"], "(", "t", ")"}]}], ")"}]}], 
              "+", 
              RowBox[{"3505943905046", " ", 
               RowBox[{"sin", "(", 
                RowBox[{
                 RowBox[{
                  SubscriptBox["\[Theta]", "3"], "(", "t", ")"}], "-", 
                 RowBox[{
                  SubscriptBox["\[Theta]", "4"], "(", "t", ")"}]}], ")"}]}], 
              "+", 
              RowBox[{"4672318239215", " ", 
               RowBox[{"sin", "(", 
                RowBox[{
                 RowBox[{
                  SubscriptBox["\[Theta]", "2"], "(", "t", ")"}], "+", 
                 RowBox[{
                  SubscriptBox["\[Theta]", "3"], "(", "t", ")"}], "-", 
                 RowBox[{
                  SubscriptBox["\[Theta]", "4"], "(", "t", ")"}]}], ")"}]}], 
              "+", 
              RowBox[{"360487780830", " ", 
               RowBox[{"sin", "(", 
                RowBox[{
                 RowBox[{
                  SubscriptBox["\[Theta]", "3"], "(", "t", ")"}], "-", 
                 RowBox[{
                  SubscriptBox["\[Theta]", "4"], "(", "t", ")"}], "-", 
                 RowBox[{
                  SubscriptBox["\[Theta]", "5"], "(", "t", ")"}]}], ")"}]}], 
              "+", 
              RowBox[{"480416595075", " ", 
               RowBox[{"sin", "(", 
                RowBox[{
                 RowBox[{
                  SubscriptBox["\[Theta]", "2"], "(", "t", ")"}], "+", 
                 RowBox[{
                  SubscriptBox["\[Theta]", "3"], "(", "t", ")"}], "-", 
                 RowBox[{
                  SubscriptBox["\[Theta]", "4"], "(", "t", ")"}], "-", 
                 RowBox[{
                  SubscriptBox["\[Theta]", "5"], "(", "t", ")"}]}], ")"}]}]}],
              ")"}], " ", 
            RowBox[{
             SuperscriptBox[
              SubscriptBox["\[Theta]", "1"], "\[Prime]",
              MultilineFunction->None], "(", "t", ")"}]}], "+", 
           RowBox[{
            RowBox[{"(", 
             RowBox[{
              RowBox[{"311561100", " ", 
               RowBox[{"cos", "(", 
                RowBox[{
                 RowBox[{
                  SubscriptBox["\[Theta]", "3"], "(", "t", ")"}], "-", 
                 RowBox[{
                  SubscriptBox["\[Theta]", "4"], "(", "t", ")"}]}], ")"}]}], 
              "+", 
              RowBox[{"415212750", " ", 
               RowBox[{"cos", "(", 
                RowBox[{
                 RowBox[{
                  SubscriptBox["\[Theta]", "2"], "(", "t", ")"}], "+", 
                 RowBox[{
                  SubscriptBox["\[Theta]", "3"], "(", "t", ")"}], "-", 
                 RowBox[{
                  SubscriptBox["\[Theta]", "4"], "(", "t", ")"}]}], ")"}]}], 
              "+", 
              RowBox[{"3505943905046", " ", 
               RowBox[{"sin", "(", 
                RowBox[{
                 RowBox[{
                  SubscriptBox["\[Theta]", "3"], "(", "t", ")"}], "-", 
                 RowBox[{
                  SubscriptBox["\[Theta]", "4"], "(", "t", ")"}]}], ")"}]}], 
              "+", 
              RowBox[{"4672318239215", " ", 
               RowBox[{"sin", "(", 
                RowBox[{
                 RowBox[{
                  SubscriptBox["\[Theta]", "2"], "(", "t", ")"}], "+", 
                 RowBox[{
                  SubscriptBox["\[Theta]", "3"], "(", "t", ")"}], "-", 
                 RowBox[{
                  SubscriptBox["\[Theta]", "4"], "(", "t", ")"}]}], ")"}]}], 
              "+", 
              RowBox[{"360487780830", " ", 
               RowBox[{"sin", "(", 
                RowBox[{
                 RowBox[{
                  SubscriptBox["\[Theta]", "3"], "(", "t", ")"}], "-", 
                 RowBox[{
                  SubscriptBox["\[Theta]", "4"], "(", "t", ")"}], "-", 
                 RowBox[{
                  SubscriptBox["\[Theta]", "5"], "(", "t", ")"}]}], ")"}]}], 
              "+", 
              RowBox[{"480416595075", " ", 
               RowBox[{"sin", "(", 
                RowBox[{
                 RowBox[{
                  SubscriptBox["\[Theta]", "2"], "(", "t", ")"}], "+", 
                 RowBox[{
                  SubscriptBox["\[Theta]", "3"], "(", "t", ")"}], "-", 
                 RowBox[{
                  SubscriptBox["\[Theta]", "4"], "(", "t", ")"}], "-", 
                 RowBox[{
                  SubscriptBox["\[Theta]", "5"], "(", "t", ")"}]}], ")"}]}]}],
              ")"}], " ", 
            RowBox[{
             SuperscriptBox[
              SubscriptBox["\[Theta]", "2"], "\[Prime]",
              MultilineFunction->None], "(", "t", ")"}]}], "+", 
           RowBox[{"311561100", " ", 
            RowBox[{"cos", "(", 
             RowBox[{
              RowBox[{
               SubscriptBox["\[Theta]", "3"], "(", "t", ")"}], "-", 
              RowBox[{
               SubscriptBox["\[Theta]", "4"], "(", "t", ")"}]}], ")"}], " ", 
            RowBox[{
             SuperscriptBox[
              SubscriptBox["\[Theta]", "3"], "\[Prime]",
              MultilineFunction->None], "(", "t", ")"}]}], "+", 
           RowBox[{"415212750", " ", 
            RowBox[{"cos", "(", 
             RowBox[{
              RowBox[{
               SubscriptBox["\[Theta]", "2"], "(", "t", ")"}], "+", 
              RowBox[{
               SubscriptBox["\[Theta]", "3"], "(", "t", ")"}], "-", 
              RowBox[{
               SubscriptBox["\[Theta]", "4"], "(", "t", ")"}]}], ")"}], " ", 
            RowBox[{
             SuperscriptBox[
              SubscriptBox["\[Theta]", "3"], "\[Prime]",
              MultilineFunction->None], "(", "t", ")"}]}], "+", 
           RowBox[{"3505943905046", " ", 
            RowBox[{"sin", "(", 
             RowBox[{
              RowBox[{
               SubscriptBox["\[Theta]", "3"], "(", "t", ")"}], "-", 
              RowBox[{
               SubscriptBox["\[Theta]", "4"], "(", "t", ")"}]}], ")"}], " ", 
            RowBox[{
             SuperscriptBox[
              SubscriptBox["\[Theta]", "3"], "\[Prime]",
              MultilineFunction->None], "(", "t", ")"}]}], "+", 
           RowBox[{"4672318239215", " ", 
            RowBox[{"sin", "(", 
             RowBox[{
              RowBox[{
               SubscriptBox["\[Theta]", "2"], "(", "t", ")"}], "+", 
              RowBox[{
               SubscriptBox["\[Theta]", "3"], "(", "t", ")"}], "-", 
              RowBox[{
               SubscriptBox["\[Theta]", "4"], "(", "t", ")"}]}], ")"}], " ", 
            RowBox[{
             SuperscriptBox[
              SubscriptBox["\[Theta]", "3"], "\[Prime]",
              MultilineFunction->None], "(", "t", ")"}]}], "+", 
           RowBox[{"360487780830", " ", 
            RowBox[{"sin", "(", 
             RowBox[{
              RowBox[{
               SubscriptBox["\[Theta]", "3"], "(", "t", ")"}], "-", 
              RowBox[{
               SubscriptBox["\[Theta]", "4"], "(", "t", ")"}], "-", 
              RowBox[{
               SubscriptBox["\[Theta]", "5"], "(", "t", ")"}]}], ")"}], " ", 
            RowBox[{
             SuperscriptBox[
              SubscriptBox["\[Theta]", "3"], "\[Prime]",
              MultilineFunction->None], "(", "t", ")"}]}], "+", 
           RowBox[{"480416595075", " ", 
            RowBox[{"sin", "(", 
             RowBox[{
              RowBox[{
               SubscriptBox["\[Theta]", "2"], "(", "t", ")"}], "+", 
              RowBox[{
               SubscriptBox["\[Theta]", "3"], "(", "t", ")"}], "-", 
              RowBox[{
               SubscriptBox["\[Theta]", "4"], "(", "t", ")"}], "-", 
              RowBox[{
               SubscriptBox["\[Theta]", "5"], "(", "t", ")"}]}], ")"}], " ", 
            RowBox[{
             SuperscriptBox[
              SubscriptBox["\[Theta]", "3"], "\[Prime]",
              MultilineFunction->None], "(", "t", ")"}]}], "-", 
           RowBox[{"311561100", " ", 
            RowBox[{"cos", "(", 
             RowBox[{
              RowBox[{
               SubscriptBox["\[Theta]", "3"], "(", "t", ")"}], "-", 
              RowBox[{
               SubscriptBox["\[Theta]", "4"], "(", "t", ")"}]}], ")"}], " ", 
            RowBox[{
             SuperscriptBox[
              SubscriptBox["\[Theta]", "4"], "\[Prime]",
              MultilineFunction->None], "(", "t", ")"}]}], "-", 
           RowBox[{"415212750", " ", 
            RowBox[{"cos", "(", 
             RowBox[{
              RowBox[{
               SubscriptBox["\[Theta]", "2"], "(", "t", ")"}], "+", 
              RowBox[{
               SubscriptBox["\[Theta]", "3"], "(", "t", ")"}], "-", 
              RowBox[{
               SubscriptBox["\[Theta]", "4"], "(", "t", ")"}]}], ")"}], " ", 
            RowBox[{
             SuperscriptBox[
              SubscriptBox["\[Theta]", "4"], "\[Prime]",
              MultilineFunction->None], "(", "t", ")"}]}], "-", 
           RowBox[{"3505943905046", " ", 
            RowBox[{"sin", "(", 
             RowBox[{
              RowBox[{
               SubscriptBox["\[Theta]", "3"], "(", "t", ")"}], "-", 
              RowBox[{
               SubscriptBox["\[Theta]", "4"], "(", "t", ")"}]}], ")"}], " ", 
            RowBox[{
             SuperscriptBox[
              SubscriptBox["\[Theta]", "4"], "\[Prime]",
              MultilineFunction->None], "(", "t", ")"}]}], "-", 
           RowBox[{"4672318239215", " ", 
            RowBox[{"sin", "(", 
             RowBox[{
              RowBox[{
               SubscriptBox["\[Theta]", "2"], "(", "t", ")"}], "+", 
              RowBox[{
               SubscriptBox["\[Theta]", "3"], "(", "t", ")"}], "-", 
              RowBox[{
               SubscriptBox["\[Theta]", "4"], "(", "t", ")"}]}], ")"}], " ", 
            RowBox[{
             SuperscriptBox[
              SubscriptBox["\[Theta]", "4"], "\[Prime]",
              MultilineFunction->None], "(", "t", ")"}]}], "-", 
           RowBox[{"360487780830", " ", 
            RowBox[{"sin", "(", 
             RowBox[{
              RowBox[{
               SubscriptBox["\[Theta]", "3"], "(", "t", ")"}], "-", 
              RowBox[{
               SubscriptBox["\[Theta]", "4"], "(", "t", ")"}], "-", 
              RowBox[{
               SubscriptBox["\[Theta]", "5"], "(", "t", ")"}]}], ")"}], " ", 
            RowBox[{
             SuperscriptBox[
              SubscriptBox["\[Theta]", "4"], "\[Prime]",
              MultilineFunction->None], "(", "t", ")"}]}], "-", 
           RowBox[{"480416595075", " ", 
            RowBox[{"sin", "(", 
             RowBox[{
              RowBox[{
               SubscriptBox["\[Theta]", "2"], "(", "t", ")"}], "+", 
              RowBox[{
               SubscriptBox["\[Theta]", "3"], "(", "t", ")"}], "-", 
              RowBox[{
               SubscriptBox["\[Theta]", "4"], "(", "t", ")"}], "-", 
              RowBox[{
               SubscriptBox["\[Theta]", "5"], "(", "t", ")"}]}], ")"}], " ", 
            RowBox[{
             SuperscriptBox[
              SubscriptBox["\[Theta]", "4"], "\[Prime]",
              MultilineFunction->None], "(", "t", ")"}]}], "-", 
           RowBox[{"360487780830", " ", 
            RowBox[{"sin", "(", 
             RowBox[{
              RowBox[{
               SubscriptBox["\[Theta]", "3"], "(", "t", ")"}], "-", 
              RowBox[{
               SubscriptBox["\[Theta]", "4"], "(", "t", ")"}], "-", 
              RowBox[{
               SubscriptBox["\[Theta]", "5"], "(", "t", ")"}]}], ")"}], " ", 
            RowBox[{
             SuperscriptBox[
              SubscriptBox["\[Theta]", "5"], "\[Prime]",
              MultilineFunction->None], "(", "t", ")"}]}], "-", 
           RowBox[{"480416595075", " ", 
            RowBox[{"sin", "(", 
             RowBox[{
              RowBox[{
               SubscriptBox["\[Theta]", "2"], "(", "t", ")"}], "+", 
              RowBox[{
               SubscriptBox["\[Theta]", "3"], "(", "t", ")"}], "-", 
              RowBox[{
               SubscriptBox["\[Theta]", "4"], "(", "t", ")"}], "-", 
              RowBox[{
               SubscriptBox["\[Theta]", "5"], "(", "t", ")"}]}], ")"}], " ", 
            RowBox[{
             SuperscriptBox[
              SubscriptBox["\[Theta]", "5"], "\[Prime]",
              MultilineFunction->None], "(", "t", ")"}]}], "-", 
           RowBox[{"360487780830", " ", 
            RowBox[{"sin", "(", 
             RowBox[{
              SubscriptBox["\[Theta]", "5"], "(", "t", ")"}], ")"}], " ", 
            RowBox[{
             SuperscriptBox[
              SubscriptBox["\[Theta]", "5"], "\[Prime]",
              MultilineFunction->None], "(", "t", ")"}]}]}], ")"}]}], 
        "12500000000000000"]}], 
      RowBox[{"-", 
       FractionBox[
        RowBox[{"17813987613", " ", 
         RowBox[{"(", 
          RowBox[{
           RowBox[{"514", " ", 
            RowBox[{"sin", "(", 
             RowBox[{
              RowBox[{
               SubscriptBox["\[Theta]", "3"], "(", "t", ")"}], "-", 
              RowBox[{
               SubscriptBox["\[Theta]", "4"], "(", "t", ")"}], "-", 
              RowBox[{
               SubscriptBox["\[Theta]", "5"], "(", "t", ")"}]}], ")"}]}], "+", 
           RowBox[{"685", " ", 
            RowBox[{"sin", "(", 
             RowBox[{
              RowBox[{
               SubscriptBox["\[Theta]", "2"], "(", "t", ")"}], "+", 
              RowBox[{
               SubscriptBox["\[Theta]", "3"], "(", "t", ")"}], "-", 
              RowBox[{
               SubscriptBox["\[Theta]", "4"], "(", "t", ")"}], "-", 
              RowBox[{
               SubscriptBox["\[Theta]", "5"], "(", "t", ")"}]}], ")"}]}], "+", 
           RowBox[{"514", " ", 
            RowBox[{"sin", "(", 
             RowBox[{
              SubscriptBox["\[Theta]", "5"], "(", "t", ")"}], ")"}]}]}], 
          ")"}], " ", 
         RowBox[{"(", 
          RowBox[{
           RowBox[{
            SuperscriptBox[
             SubscriptBox["\[Theta]", "1"], "\[Prime]",
             MultilineFunction->None], "(", "t", ")"}], "+", 
           RowBox[{
            SuperscriptBox[
             SubscriptBox["\[Theta]", "2"], "\[Prime]",
             MultilineFunction->None], "(", "t", ")"}], "+", 
           RowBox[{
            SuperscriptBox[
             SubscriptBox["\[Theta]", "3"], "\[Prime]",
             MultilineFunction->None], "(", "t", ")"}], "-", 
           RowBox[{
            SuperscriptBox[
             SubscriptBox["\[Theta]", "4"], "\[Prime]",
             MultilineFunction->None], "(", "t", ")"}], "-", 
           RowBox[{
            SuperscriptBox[
             SubscriptBox["\[Theta]", "5"], "\[Prime]",
             MultilineFunction->None], "(", "t", ")"}]}], ")"}]}], 
        "2500000000000000"]}]},
     {
      RowBox[{"-", 
       FractionBox[
        RowBox[{"127", " ", 
         RowBox[{"(", 
          RowBox[{
           RowBox[{"685", " ", 
            RowBox[{"(", 
             RowBox[{
              RowBox[{"606150", " ", 
               RowBox[{"cos", "(", 
                RowBox[{
                 SubscriptBox["\[Theta]", "2"], "(", "t", ")"}], ")"}]}], "-", 
              RowBox[{"9657960", " ", 
               RowBox[{"cos", "(", 
                RowBox[{
                 RowBox[{
                  SubscriptBox["\[Theta]", "2"], "(", "t", ")"}], "+", 
                 RowBox[{
                  SubscriptBox["\[Theta]", "3"], "(", "t", ")"}]}], ")"}]}], 
              "+", 
              RowBox[{"606150", " ", 
               RowBox[{"cos", "(", 
                RowBox[{
                 RowBox[{
                  SubscriptBox["\[Theta]", "2"], "(", "t", ")"}], "+", 
                 RowBox[{
                  SubscriptBox["\[Theta]", "3"], "(", "t", ")"}], "-", 
                 RowBox[{
                  SubscriptBox["\[Theta]", "4"], "(", "t", ")"}]}], ")"}]}], 
              "-", 
              RowBox[{"25096253337", " ", 
               RowBox[{"sin", "(", 
                RowBox[{
                 SubscriptBox["\[Theta]", "2"], "(", "t", ")"}], ")"}]}], "-", 
              RowBox[{"401207755", " ", 
               RowBox[{"sin", "(", 
                RowBox[{
                 RowBox[{
                  SubscriptBox["\[Theta]", "2"], "(", "t", ")"}], "+", 
                 RowBox[{
                  SubscriptBox["\[Theta]", "3"], "(", "t", ")"}]}], ")"}]}], 
              "+", 
              RowBox[{"6820902539", " ", 
               RowBox[{"sin", "(", 
                RowBox[{
                 RowBox[{
                  SubscriptBox["\[Theta]", "2"], "(", "t", ")"}], "+", 
                 RowBox[{
                  SubscriptBox["\[Theta]", "3"], "(", "t", ")"}], "-", 
                 RowBox[{
                  SubscriptBox["\[Theta]", "4"], "(", "t", ")"}]}], ")"}]}], 
              "+", 
              RowBox[{"701338095", " ", 
               RowBox[{"sin", "(", 
                RowBox[{
                 RowBox[{
                  SubscriptBox["\[Theta]", "2"], "(", "t", ")"}], "+", 
                 RowBox[{
                  SubscriptBox["\[Theta]", "3"], "(", "t", ")"}], "-", 
                 RowBox[{
                  SubscriptBox["\[Theta]", "4"], "(", "t", ")"}], "-", 
                 RowBox[{
                  SubscriptBox["\[Theta]", "5"], "(", "t", ")"}]}], ")"}]}]}],
              ")"}], " ", 
            RowBox[{
             SuperscriptBox[
              SubscriptBox["\[Theta]", "1"], "\[Prime]",
              MultilineFunction->None], "(", "t", ")"}]}], "+", 
           RowBox[{"514", " ", 
            RowBox[{"(", 
             RowBox[{
              RowBox[{
               RowBox[{"(", 
                RowBox[{
                 RowBox[{"9657960", " ", 
                  RowBox[{"cos", "(", 
                   RowBox[{
                    SubscriptBox["\[Theta]", "3"], "(", "t", ")"}], ")"}]}], 
                 "-", 
                 RowBox[{"606150", " ", 
                  RowBox[{"cos", "(", 
                   RowBox[{
                    RowBox[{
                    SubscriptBox["\[Theta]", "3"], "(", "t", ")"}], "-", 
                    RowBox[{
                    SubscriptBox["\[Theta]", "4"], "(", "t", ")"}]}], ")"}]}],
                  "+", 
                 RowBox[{"401207755", " ", 
                  RowBox[{"sin", "(", 
                   RowBox[{
                    SubscriptBox["\[Theta]", "3"], "(", "t", ")"}], ")"}]}], 
                 "-", 
                 RowBox[{"6820902539", " ", 
                  RowBox[{"sin", "(", 
                   RowBox[{
                    RowBox[{
                    SubscriptBox["\[Theta]", "3"], "(", "t", ")"}], "-", 
                    RowBox[{
                    SubscriptBox["\[Theta]", "4"], "(", "t", ")"}]}], ")"}]}],
                  "-", 
                 RowBox[{"701338095", " ", 
                  RowBox[{"sin", "(", 
                   RowBox[{
                    RowBox[{
                    SubscriptBox["\[Theta]", "3"], "(", "t", ")"}], "-", 
                    RowBox[{
                    SubscriptBox["\[Theta]", "4"], "(", "t", ")"}], "-", 
                    RowBox[{
                    SubscriptBox["\[Theta]", "5"], "(", "t", ")"}]}], 
                   ")"}]}]}], ")"}], " ", 
               RowBox[{
                SuperscriptBox[
                 SubscriptBox["\[Theta]", "3"], "\[Prime]",
                 MultilineFunction->None], "(", "t", ")"}]}], "+", 
              RowBox[{
               RowBox[{"(", 
                RowBox[{
                 RowBox[{"606150", " ", 
                  RowBox[{"cos", "(", 
                   RowBox[{
                    RowBox[{
                    SubscriptBox["\[Theta]", "3"], "(", "t", ")"}], "-", 
                    RowBox[{
                    SubscriptBox["\[Theta]", "4"], "(", "t", ")"}]}], ")"}]}],
                  "+", 
                 RowBox[{"6820902539", " ", 
                  RowBox[{"sin", "(", 
                   RowBox[{
                    RowBox[{
                    SubscriptBox["\[Theta]", "3"], "(", "t", ")"}], "-", 
                    RowBox[{
                    SubscriptBox["\[Theta]", "4"], "(", "t", ")"}]}], ")"}]}],
                  "+", 
                 RowBox[{"701338095", " ", 
                  RowBox[{"sin", "(", 
                   RowBox[{
                    RowBox[{
                    SubscriptBox["\[Theta]", "3"], "(", "t", ")"}], "-", 
                    RowBox[{
                    SubscriptBox["\[Theta]", "4"], "(", "t", ")"}], "-", 
                    RowBox[{
                    SubscriptBox["\[Theta]", "5"], "(", "t", ")"}]}], 
                   ")"}]}]}], ")"}], " ", 
               RowBox[{
                SuperscriptBox[
                 SubscriptBox["\[Theta]", "4"], "\[Prime]",
                 MultilineFunction->None], "(", "t", ")"}]}], "+", 
              RowBox[{"701338095", " ", 
               RowBox[{"(", 
                RowBox[{
                 RowBox[{"sin", "(", 
                  RowBox[{
                   RowBox[{
                    SubscriptBox["\[Theta]", "3"], "(", "t", ")"}], "-", 
                   RowBox[{
                    SubscriptBox["\[Theta]", "4"], "(", "t", ")"}], "-", 
                   RowBox[{
                    SubscriptBox["\[Theta]", "5"], "(", "t", ")"}]}], ")"}], 
                 "+", 
                 RowBox[{"sin", "(", 
                  RowBox[{
                   SubscriptBox["\[Theta]", "5"], "(", "t", ")"}], ")"}]}], 
                ")"}], " ", 
               RowBox[{
                SuperscriptBox[
                 SubscriptBox["\[Theta]", "5"], "\[Prime]",
                 MultilineFunction->None], "(", "t", ")"}]}]}], ")"}]}]}], 
          ")"}]}], "12500000000000000"]}], 
      RowBox[{"-", 
       FractionBox[
        RowBox[{"32639", " ", 
         RowBox[{"(", 
          RowBox[{
           RowBox[{
            RowBox[{"(", 
             RowBox[{
              RowBox[{"9657960", " ", 
               RowBox[{"cos", "(", 
                RowBox[{
                 SubscriptBox["\[Theta]", "3"], "(", "t", ")"}], ")"}]}], "-", 
              RowBox[{"606150", " ", 
               RowBox[{"cos", "(", 
                RowBox[{
                 RowBox[{
                  SubscriptBox["\[Theta]", "3"], "(", "t", ")"}], "-", 
                 RowBox[{
                  SubscriptBox["\[Theta]", "4"], "(", "t", ")"}]}], ")"}]}], 
              "+", 
              RowBox[{"401207755", " ", 
               RowBox[{"sin", "(", 
                RowBox[{
                 SubscriptBox["\[Theta]", "3"], "(", "t", ")"}], ")"}]}], "-", 
              RowBox[{"6820902539", " ", 
               RowBox[{"sin", "(", 
                RowBox[{
                 RowBox[{
                  SubscriptBox["\[Theta]", "3"], "(", "t", ")"}], "-", 
                 RowBox[{
                  SubscriptBox["\[Theta]", "4"], "(", "t", ")"}]}], ")"}]}], 
              "-", 
              RowBox[{"701338095", " ", 
               RowBox[{"sin", "(", 
                RowBox[{
                 RowBox[{
                  SubscriptBox["\[Theta]", "3"], "(", "t", ")"}], "-", 
                 RowBox[{
                  SubscriptBox["\[Theta]", "4"], "(", "t", ")"}], "-", 
                 RowBox[{
                  SubscriptBox["\[Theta]", "5"], "(", "t", ")"}]}], ")"}]}]}],
              ")"}], " ", 
            RowBox[{
             SuperscriptBox[
              SubscriptBox["\[Theta]", "3"], "\[Prime]",
              MultilineFunction->None], "(", "t", ")"}]}], "+", 
           RowBox[{
            RowBox[{"(", 
             RowBox[{
              RowBox[{"606150", " ", 
               RowBox[{"cos", "(", 
                RowBox[{
                 RowBox[{
                  SubscriptBox["\[Theta]", "3"], "(", "t", ")"}], "-", 
                 RowBox[{
                  SubscriptBox["\[Theta]", "4"], "(", "t", ")"}]}], ")"}]}], 
              "+", 
              RowBox[{"6820902539", " ", 
               RowBox[{"sin", "(", 
                RowBox[{
                 RowBox[{
                  SubscriptBox["\[Theta]", "3"], "(", "t", ")"}], "-", 
                 RowBox[{
                  SubscriptBox["\[Theta]", "4"], "(", "t", ")"}]}], ")"}]}], 
              "+", 
              RowBox[{"701338095", " ", 
               RowBox[{"sin", "(", 
                RowBox[{
                 RowBox[{
                  SubscriptBox["\[Theta]", "3"], "(", "t", ")"}], "-", 
                 RowBox[{
                  SubscriptBox["\[Theta]", "4"], "(", "t", ")"}], "-", 
                 RowBox[{
                  SubscriptBox["\[Theta]", "5"], "(", "t", ")"}]}], ")"}]}]}],
              ")"}], " ", 
            RowBox[{
             SuperscriptBox[
              SubscriptBox["\[Theta]", "4"], "\[Prime]",
              MultilineFunction->None], "(", "t", ")"}]}], "+", 
           RowBox[{"701338095", " ", 
            RowBox[{"(", 
             RowBox[{
              RowBox[{"sin", "(", 
               RowBox[{
                RowBox[{
                 SubscriptBox["\[Theta]", "3"], "(", "t", ")"}], "-", 
                RowBox[{
                 SubscriptBox["\[Theta]", "4"], "(", "t", ")"}], "-", 
                RowBox[{
                 SubscriptBox["\[Theta]", "5"], "(", "t", ")"}]}], ")"}], "+", 
              RowBox[{"sin", "(", 
               RowBox[{
                SubscriptBox["\[Theta]", "5"], "(", "t", ")"}], ")"}]}], 
             ")"}], " ", 
            RowBox[{
             SuperscriptBox[
              SubscriptBox["\[Theta]", "5"], "\[Prime]",
              MultilineFunction->None], "(", "t", ")"}]}]}], ")"}]}], 
        "6250000000000000"]}], 
      RowBox[{"-", 
       FractionBox[
        RowBox[{"32639", " ", 
         RowBox[{"(", 
          RowBox[{
           RowBox[{
            RowBox[{"(", 
             RowBox[{
              RowBox[{"9657960", " ", 
               RowBox[{"cos", "(", 
                RowBox[{
                 SubscriptBox["\[Theta]", "3"], "(", "t", ")"}], ")"}]}], "-", 
              RowBox[{"606150", " ", 
               RowBox[{"cos", "(", 
                RowBox[{
                 RowBox[{
                  SubscriptBox["\[Theta]", "3"], "(", "t", ")"}], "-", 
                 RowBox[{
                  SubscriptBox["\[Theta]", "4"], "(", "t", ")"}]}], ")"}]}], 
              "+", 
              RowBox[{"401207755", " ", 
               RowBox[{"sin", "(", 
                RowBox[{
                 SubscriptBox["\[Theta]", "3"], "(", "t", ")"}], ")"}]}], "-", 
              RowBox[{"6820902539", " ", 
               RowBox[{"sin", "(", 
                RowBox[{
                 RowBox[{
                  SubscriptBox["\[Theta]", "3"], "(", "t", ")"}], "-", 
                 RowBox[{
                  SubscriptBox["\[Theta]", "4"], "(", "t", ")"}]}], ")"}]}], 
              "-", 
              RowBox[{"701338095", " ", 
               RowBox[{"sin", "(", 
                RowBox[{
                 RowBox[{
                  SubscriptBox["\[Theta]", "3"], "(", "t", ")"}], "-", 
                 RowBox[{
                  SubscriptBox["\[Theta]", "4"], "(", "t", ")"}], "-", 
                 RowBox[{
                  SubscriptBox["\[Theta]", "5"], "(", "t", ")"}]}], ")"}]}]}],
              ")"}], " ", 
            RowBox[{
             SuperscriptBox[
              SubscriptBox["\[Theta]", "1"], "\[Prime]",
              MultilineFunction->None], "(", "t", ")"}]}], "+", 
           RowBox[{
            RowBox[{"(", 
             RowBox[{
              RowBox[{"9657960", " ", 
               RowBox[{"cos", "(", 
                RowBox[{
                 SubscriptBox["\[Theta]", "3"], "(", "t", ")"}], ")"}]}], "-", 
              RowBox[{"606150", " ", 
               RowBox[{"cos", "(", 
                RowBox[{
                 RowBox[{
                  SubscriptBox["\[Theta]", "3"], "(", "t", ")"}], "-", 
                 RowBox[{
                  SubscriptBox["\[Theta]", "4"], "(", "t", ")"}]}], ")"}]}], 
              "+", 
              RowBox[{"401207755", " ", 
               RowBox[{"sin", "(", 
                RowBox[{
                 SubscriptBox["\[Theta]", "3"], "(", "t", ")"}], ")"}]}], "-", 
              RowBox[{"6820902539", " ", 
               RowBox[{"sin", "(", 
                RowBox[{
                 RowBox[{
                  SubscriptBox["\[Theta]", "3"], "(", "t", ")"}], "-", 
                 RowBox[{
                  SubscriptBox["\[Theta]", "4"], "(", "t", ")"}]}], ")"}]}], 
              "-", 
              RowBox[{"701338095", " ", 
               RowBox[{"sin", "(", 
                RowBox[{
                 RowBox[{
                  SubscriptBox["\[Theta]", "3"], "(", "t", ")"}], "-", 
                 RowBox[{
                  SubscriptBox["\[Theta]", "4"], "(", "t", ")"}], "-", 
                 RowBox[{
                  SubscriptBox["\[Theta]", "5"], "(", "t", ")"}]}], ")"}]}]}],
              ")"}], " ", 
            RowBox[{
             SuperscriptBox[
              SubscriptBox["\[Theta]", "2"], "\[Prime]",
              MultilineFunction->None], "(", "t", ")"}]}], "+", 
           RowBox[{"9657960", " ", 
            RowBox[{"cos", "(", 
             RowBox[{
              SubscriptBox["\[Theta]", "3"], "(", "t", ")"}], ")"}], " ", 
            RowBox[{
             SuperscriptBox[
              SubscriptBox["\[Theta]", "3"], "\[Prime]",
              MultilineFunction->None], "(", "t", ")"}]}], "-", 
           RowBox[{"606150", " ", 
            RowBox[{"cos", "(", 
             RowBox[{
              RowBox[{
               SubscriptBox["\[Theta]", "3"], "(", "t", ")"}], "-", 
              RowBox[{
               SubscriptBox["\[Theta]", "4"], "(", "t", ")"}]}], ")"}], " ", 
            RowBox[{
             SuperscriptBox[
              SubscriptBox["\[Theta]", "3"], "\[Prime]",
              MultilineFunction->None], "(", "t", ")"}]}], "+", 
           RowBox[{"401207755", " ", 
            RowBox[{"sin", "(", 
             RowBox[{
              SubscriptBox["\[Theta]", "3"], "(", "t", ")"}], ")"}], " ", 
            RowBox[{
             SuperscriptBox[
              SubscriptBox["\[Theta]", "3"], "\[Prime]",
              MultilineFunction->None], "(", "t", ")"}]}], "-", 
           RowBox[{"6820902539", " ", 
            RowBox[{"sin", "(", 
             RowBox[{
              RowBox[{
               SubscriptBox["\[Theta]", "3"], "(", "t", ")"}], "-", 
              RowBox[{
               SubscriptBox["\[Theta]", "4"], "(", "t", ")"}]}], ")"}], " ", 
            RowBox[{
             SuperscriptBox[
              SubscriptBox["\[Theta]", "3"], "\[Prime]",
              MultilineFunction->None], "(", "t", ")"}]}], "-", 
           RowBox[{"701338095", " ", 
            RowBox[{"sin", "(", 
             RowBox[{
              RowBox[{
               SubscriptBox["\[Theta]", "3"], "(", "t", ")"}], "-", 
              RowBox[{
               SubscriptBox["\[Theta]", "4"], "(", "t", ")"}], "-", 
              RowBox[{
               SubscriptBox["\[Theta]", "5"], "(", "t", ")"}]}], ")"}], " ", 
            RowBox[{
             SuperscriptBox[
              SubscriptBox["\[Theta]", "3"], "\[Prime]",
              MultilineFunction->None], "(", "t", ")"}]}], "+", 
           RowBox[{"606150", " ", 
            RowBox[{"cos", "(", 
             RowBox[{
              RowBox[{
               SubscriptBox["\[Theta]", "3"], "(", "t", ")"}], "-", 
              RowBox[{
               SubscriptBox["\[Theta]", "4"], "(", "t", ")"}]}], ")"}], " ", 
            RowBox[{
             SuperscriptBox[
              SubscriptBox["\[Theta]", "4"], "\[Prime]",
              MultilineFunction->None], "(", "t", ")"}]}], "+", 
           RowBox[{"6820902539", " ", 
            RowBox[{"sin", "(", 
             RowBox[{
              RowBox[{
               SubscriptBox["\[Theta]", "3"], "(", "t", ")"}], "-", 
              RowBox[{
               SubscriptBox["\[Theta]", "4"], "(", "t", ")"}]}], ")"}], " ", 
            RowBox[{
             SuperscriptBox[
              SubscriptBox["\[Theta]", "4"], "\[Prime]",
              MultilineFunction->None], "(", "t", ")"}]}], "+", 
           RowBox[{"701338095", " ", 
            RowBox[{"sin", "(", 
             RowBox[{
              RowBox[{
               SubscriptBox["\[Theta]", "3"], "(", "t", ")"}], "-", 
              RowBox[{
               SubscriptBox["\[Theta]", "4"], "(", "t", ")"}], "-", 
              RowBox[{
               SubscriptBox["\[Theta]", "5"], "(", "t", ")"}]}], ")"}], " ", 
            RowBox[{
             SuperscriptBox[
              SubscriptBox["\[Theta]", "4"], "\[Prime]",
              MultilineFunction->None], "(", "t", ")"}]}], "+", 
           RowBox[{"701338095", " ", 
            RowBox[{"sin", "(", 
             RowBox[{
              RowBox[{
               SubscriptBox["\[Theta]", "3"], "(", "t", ")"}], "-", 
              RowBox[{
               SubscriptBox["\[Theta]", "4"], "(", "t", ")"}], "-", 
              RowBox[{
               SubscriptBox["\[Theta]", "5"], "(", "t", ")"}]}], ")"}], " ", 
            RowBox[{
             SuperscriptBox[
              SubscriptBox["\[Theta]", "5"], "\[Prime]",
              MultilineFunction->None], "(", "t", ")"}]}], "+", 
           RowBox[{"701338095", " ", 
            RowBox[{"sin", "(", 
             RowBox[{
              SubscriptBox["\[Theta]", "5"], "(", "t", ")"}], ")"}], " ", 
            RowBox[{
             SuperscriptBox[
              SubscriptBox["\[Theta]", "5"], "\[Prime]",
              MultilineFunction->None], "(", "t", ")"}]}]}], ")"}]}], 
        "6250000000000000"]}], 
      RowBox[{"-", 
       FractionBox[
        RowBox[{"32639", " ", 
         RowBox[{"(", 
          RowBox[{
           RowBox[{
            RowBox[{"(", 
             RowBox[{
              RowBox[{"606150", " ", 
               RowBox[{"cos", "(", 
                RowBox[{
                 RowBox[{
                  SubscriptBox["\[Theta]", "3"], "(", "t", ")"}], "-", 
                 RowBox[{
                  SubscriptBox["\[Theta]", "4"], "(", "t", ")"}]}], ")"}]}], 
              "+", 
              RowBox[{"6820902539", " ", 
               RowBox[{"sin", "(", 
                RowBox[{
                 RowBox[{
                  SubscriptBox["\[Theta]", "3"], "(", "t", ")"}], "-", 
                 RowBox[{
                  SubscriptBox["\[Theta]", "4"], "(", "t", ")"}]}], ")"}]}], 
              "+", 
              RowBox[{"701338095", " ", 
               RowBox[{"sin", "(", 
                RowBox[{
                 RowBox[{
                  SubscriptBox["\[Theta]", "3"], "(", "t", ")"}], "-", 
                 RowBox[{
                  SubscriptBox["\[Theta]", "4"], "(", "t", ")"}], "-", 
                 RowBox[{
                  SubscriptBox["\[Theta]", "5"], "(", "t", ")"}]}], ")"}]}]}],
              ")"}], " ", 
            RowBox[{
             SuperscriptBox[
              SubscriptBox["\[Theta]", "1"], "\[Prime]",
              MultilineFunction->None], "(", "t", ")"}]}], "+", 
           RowBox[{
            RowBox[{"(", 
             RowBox[{
              RowBox[{"606150", " ", 
               RowBox[{"cos", "(", 
                RowBox[{
                 RowBox[{
                  SubscriptBox["\[Theta]", "3"], "(", "t", ")"}], "-", 
                 RowBox[{
                  SubscriptBox["\[Theta]", "4"], "(", "t", ")"}]}], ")"}]}], 
              "+", 
              RowBox[{"6820902539", " ", 
               RowBox[{"sin", "(", 
                RowBox[{
                 RowBox[{
                  SubscriptBox["\[Theta]", "3"], "(", "t", ")"}], "-", 
                 RowBox[{
                  SubscriptBox["\[Theta]", "4"], "(", "t", ")"}]}], ")"}]}], 
              "+", 
              RowBox[{"701338095", " ", 
               RowBox[{"sin", "(", 
                RowBox[{
                 RowBox[{
                  SubscriptBox["\[Theta]", "3"], "(", "t", ")"}], "-", 
                 RowBox[{
                  SubscriptBox["\[Theta]", "4"], "(", "t", ")"}], "-", 
                 RowBox[{
                  SubscriptBox["\[Theta]", "5"], "(", "t", ")"}]}], ")"}]}]}],
              ")"}], " ", 
            RowBox[{
             SuperscriptBox[
              SubscriptBox["\[Theta]", "2"], "\[Prime]",
              MultilineFunction->None], "(", "t", ")"}]}], "+", 
           RowBox[{"606150", " ", 
            RowBox[{"cos", "(", 
             RowBox[{
              RowBox[{
               SubscriptBox["\[Theta]", "3"], "(", "t", ")"}], "-", 
              RowBox[{
               SubscriptBox["\[Theta]", "4"], "(", "t", ")"}]}], ")"}], " ", 
            RowBox[{
             SuperscriptBox[
              SubscriptBox["\[Theta]", "3"], "\[Prime]",
              MultilineFunction->None], "(", "t", ")"}]}], "+", 
           RowBox[{"6820902539", " ", 
            RowBox[{"sin", "(", 
             RowBox[{
              RowBox[{
               SubscriptBox["\[Theta]", "3"], "(", "t", ")"}], "-", 
              RowBox[{
               SubscriptBox["\[Theta]", "4"], "(", "t", ")"}]}], ")"}], " ", 
            RowBox[{
             SuperscriptBox[
              SubscriptBox["\[Theta]", "3"], "\[Prime]",
              MultilineFunction->None], "(", "t", ")"}]}], "+", 
           RowBox[{"701338095", " ", 
            RowBox[{"sin", "(", 
             RowBox[{
              RowBox[{
               SubscriptBox["\[Theta]", "3"], "(", "t", ")"}], "-", 
              RowBox[{
               SubscriptBox["\[Theta]", "4"], "(", "t", ")"}], "-", 
              RowBox[{
               SubscriptBox["\[Theta]", "5"], "(", "t", ")"}]}], ")"}], " ", 
            RowBox[{
             SuperscriptBox[
              SubscriptBox["\[Theta]", "3"], "\[Prime]",
              MultilineFunction->None], "(", "t", ")"}]}], "-", 
           RowBox[{"606150", " ", 
            RowBox[{"cos", "(", 
             RowBox[{
              RowBox[{
               SubscriptBox["\[Theta]", "3"], "(", "t", ")"}], "-", 
              RowBox[{
               SubscriptBox["\[Theta]", "4"], "(", "t", ")"}]}], ")"}], " ", 
            RowBox[{
             SuperscriptBox[
              SubscriptBox["\[Theta]", "4"], "\[Prime]",
              MultilineFunction->None], "(", "t", ")"}]}], "-", 
           RowBox[{"6820902539", " ", 
            RowBox[{"sin", "(", 
             RowBox[{
              RowBox[{
               SubscriptBox["\[Theta]", "3"], "(", "t", ")"}], "-", 
              RowBox[{
               SubscriptBox["\[Theta]", "4"], "(", "t", ")"}]}], ")"}], " ", 
            RowBox[{
             SuperscriptBox[
              SubscriptBox["\[Theta]", "4"], "\[Prime]",
              MultilineFunction->None], "(", "t", ")"}]}], "-", 
           RowBox[{"701338095", " ", 
            RowBox[{"sin", "(", 
             RowBox[{
              RowBox[{
               SubscriptBox["\[Theta]", "3"], "(", "t", ")"}], "-", 
              RowBox[{
               SubscriptBox["\[Theta]", "4"], "(", "t", ")"}], "-", 
              RowBox[{
               SubscriptBox["\[Theta]", "5"], "(", "t", ")"}]}], ")"}], " ", 
            RowBox[{
             SuperscriptBox[
              SubscriptBox["\[Theta]", "4"], "\[Prime]",
              MultilineFunction->None], "(", "t", ")"}]}], "-", 
           RowBox[{"701338095", " ", 
            RowBox[{"sin", "(", 
             RowBox[{
              RowBox[{
               SubscriptBox["\[Theta]", "3"], "(", "t", ")"}], "-", 
              RowBox[{
               SubscriptBox["\[Theta]", "4"], "(", "t", ")"}], "-", 
              RowBox[{
               SubscriptBox["\[Theta]", "5"], "(", "t", ")"}]}], ")"}], " ", 
            RowBox[{
             SuperscriptBox[
              SubscriptBox["\[Theta]", "5"], "\[Prime]",
              MultilineFunction->None], "(", "t", ")"}]}], "-", 
           RowBox[{"701338095", " ", 
            RowBox[{"sin", "(", 
             RowBox[{
              SubscriptBox["\[Theta]", "5"], "(", "t", ")"}], ")"}], " ", 
            RowBox[{
             SuperscriptBox[
              SubscriptBox["\[Theta]", "5"], "\[Prime]",
              MultilineFunction->None], "(", "t", ")"}]}]}], ")"}]}], 
        "6250000000000000"]}], 
      RowBox[{"-", 
       FractionBox[
        RowBox[{"4578194816541", " ", 
         RowBox[{"cos", "(", 
          RowBox[{
           FractionBox["1", "2"], " ", 
           RowBox[{"(", 
            RowBox[{
             RowBox[{
              SubscriptBox["\[Theta]", "3"], "(", "t", ")"}], "-", 
             RowBox[{
              SubscriptBox["\[Theta]", "4"], "(", "t", ")"}], "-", 
             RowBox[{"2", " ", 
              RowBox[{
               SubscriptBox["\[Theta]", "5"], "(", "t", ")"}]}]}], ")"}]}], 
          ")"}], " ", 
         RowBox[{"sin", "(", 
          RowBox[{
           FractionBox["1", "2"], " ", 
           RowBox[{"(", 
            RowBox[{
             RowBox[{
              SubscriptBox["\[Theta]", "3"], "(", "t", ")"}], "-", 
             RowBox[{
              SubscriptBox["\[Theta]", "4"], "(", "t", ")"}]}], ")"}]}], 
          ")"}], " ", 
         RowBox[{"(", 
          RowBox[{
           RowBox[{
            SuperscriptBox[
             SubscriptBox["\[Theta]", "1"], "\[Prime]",
             MultilineFunction->None], "(", "t", ")"}], "+", 
           RowBox[{
            SuperscriptBox[
             SubscriptBox["\[Theta]", "2"], "\[Prime]",
             MultilineFunction->None], "(", "t", ")"}], "+", 
           RowBox[{
            SuperscriptBox[
             SubscriptBox["\[Theta]", "3"], "\[Prime]",
             MultilineFunction->None], "(", "t", ")"}], "-", 
           RowBox[{
            SuperscriptBox[
             SubscriptBox["\[Theta]", "4"], "\[Prime]",
             MultilineFunction->None], "(", "t", ")"}], "-", 
           RowBox[{
            SuperscriptBox[
             SubscriptBox["\[Theta]", "5"], "\[Prime]",
             MultilineFunction->None], "(", "t", ")"}]}], ")"}]}], 
        "625000000000000"]}]},
     {
      RowBox[{"-", 
       FractionBox[
        RowBox[{"127", " ", 
         RowBox[{"(", 
          RowBox[{
           RowBox[{
            RowBox[{"(", 
             RowBox[{
              RowBox[{
               RowBox[{"-", "4964191440"}], " ", 
               RowBox[{"cos", "(", 
                RowBox[{
                 SubscriptBox["\[Theta]", "3"], "(", "t", ")"}], ")"}]}], "-", 
              RowBox[{"6615702600", " ", 
               RowBox[{"cos", "(", 
                RowBox[{
                 RowBox[{
                  SubscriptBox["\[Theta]", "2"], "(", "t", ")"}], "+", 
                 RowBox[{
                  SubscriptBox["\[Theta]", "3"], "(", "t", ")"}]}], ")"}]}], 
              "+", 
              RowBox[{"311561100", " ", 
               RowBox[{"cos", "(", 
                RowBox[{
                 RowBox[{
                  SubscriptBox["\[Theta]", "3"], "(", "t", ")"}], "-", 
                 RowBox[{
                  SubscriptBox["\[Theta]", "4"], "(", "t", ")"}]}], ")"}]}], 
              "+", 
              RowBox[{"415212750", " ", 
               RowBox[{"cos", "(", 
                RowBox[{
                 RowBox[{
                  SubscriptBox["\[Theta]", "2"], "(", "t", ")"}], "+", 
                 RowBox[{
                  SubscriptBox["\[Theta]", "3"], "(", "t", ")"}], "-", 
                 RowBox[{
                  SubscriptBox["\[Theta]", "4"], "(", "t", ")"}]}], ")"}]}], 
              "-", 
              RowBox[{"206220786070", " ", 
               RowBox[{"sin", "(", 
                RowBox[{
                 SubscriptBox["\[Theta]", "3"], "(", "t", ")"}], ")"}]}], "-", 
              RowBox[{"274827312175", " ", 
               RowBox[{"sin", "(", 
                RowBox[{
                 RowBox[{
                  SubscriptBox["\[Theta]", "2"], "(", "t", ")"}], "+", 
                 RowBox[{
                  SubscriptBox["\[Theta]", "3"], "(", "t", ")"}]}], ")"}]}], 
              "+", 
              RowBox[{"3505943905046", " ", 
               RowBox[{"sin", "(", 
                RowBox[{
                 RowBox[{
                  SubscriptBox["\[Theta]", "3"], "(", "t", ")"}], "-", 
                 RowBox[{
                  SubscriptBox["\[Theta]", "4"], "(", "t", ")"}]}], ")"}]}], 
              "+", 
              RowBox[{"4672318239215", " ", 
               RowBox[{"sin", "(", 
                RowBox[{
                 RowBox[{
                  SubscriptBox["\[Theta]", "2"], "(", "t", ")"}], "+", 
                 RowBox[{
                  SubscriptBox["\[Theta]", "3"], "(", "t", ")"}], "-", 
                 RowBox[{
                  SubscriptBox["\[Theta]", "4"], "(", "t", ")"}]}], ")"}]}], 
              "+", 
              RowBox[{"360487780830", " ", 
               RowBox[{"sin", "(", 
                RowBox[{
                 RowBox[{
                  SubscriptBox["\[Theta]", "3"], "(", "t", ")"}], "-", 
                 RowBox[{
                  SubscriptBox["\[Theta]", "4"], "(", "t", ")"}], "-", 
                 RowBox[{
                  SubscriptBox["\[Theta]", "5"], "(", "t", ")"}]}], ")"}]}], 
              "+", 
              RowBox[{"480416595075", " ", 
               RowBox[{"sin", "(", 
                RowBox[{
                 RowBox[{
                  SubscriptBox["\[Theta]", "2"], "(", "t", ")"}], "+", 
                 RowBox[{
                  SubscriptBox["\[Theta]", "3"], "(", "t", ")"}], "-", 
                 RowBox[{
                  SubscriptBox["\[Theta]", "4"], "(", "t", ")"}], "-", 
                 RowBox[{
                  SubscriptBox["\[Theta]", "5"], "(", "t", ")"}]}], ")"}]}]}],
              ")"}], " ", 
            RowBox[{
             SuperscriptBox[
              SubscriptBox["\[Theta]", "1"], "\[Prime]",
              MultilineFunction->None], "(", "t", ")"}]}], "+", 
           RowBox[{"514", " ", 
            RowBox[{"(", 
             RowBox[{
              RowBox[{
               RowBox[{"(", 
                RowBox[{
                 RowBox[{
                  RowBox[{"-", "9657960"}], " ", 
                  RowBox[{"cos", "(", 
                   RowBox[{
                    SubscriptBox["\[Theta]", "3"], "(", "t", ")"}], ")"}]}], 
                 "+", 
                 RowBox[{"606150", " ", 
                  RowBox[{"cos", "(", 
                   RowBox[{
                    RowBox[{
                    SubscriptBox["\[Theta]", "3"], "(", "t", ")"}], "-", 
                    RowBox[{
                    SubscriptBox["\[Theta]", "4"], "(", "t", ")"}]}], ")"}]}],
                  "-", 
                 RowBox[{"401207755", " ", 
                  RowBox[{"sin", "(", 
                   RowBox[{
                    SubscriptBox["\[Theta]", "3"], "(", "t", ")"}], ")"}]}], 
                 "+", 
                 RowBox[{"6820902539", " ", 
                  RowBox[{"sin", "(", 
                   RowBox[{
                    RowBox[{
                    SubscriptBox["\[Theta]", "3"], "(", "t", ")"}], "-", 
                    RowBox[{
                    SubscriptBox["\[Theta]", "4"], "(", "t", ")"}]}], ")"}]}],
                  "+", 
                 RowBox[{"701338095", " ", 
                  RowBox[{"sin", "(", 
                   RowBox[{
                    RowBox[{
                    SubscriptBox["\[Theta]", "3"], "(", "t", ")"}], "-", 
                    RowBox[{
                    SubscriptBox["\[Theta]", "4"], "(", "t", ")"}], "-", 
                    RowBox[{
                    SubscriptBox["\[Theta]", "5"], "(", "t", ")"}]}], 
                   ")"}]}]}], ")"}], " ", 
               RowBox[{
                SuperscriptBox[
                 SubscriptBox["\[Theta]", "2"], "\[Prime]",
                 MultilineFunction->None], "(", "t", ")"}]}], "+", 
              RowBox[{"701338095", " ", 
               RowBox[{"sin", "(", 
                RowBox[{
                 SubscriptBox["\[Theta]", "5"], "(", "t", ")"}], ")"}], " ", 
               RowBox[{
                SuperscriptBox[
                 SubscriptBox["\[Theta]", "5"], "\[Prime]",
                 MultilineFunction->None], "(", "t", ")"}]}]}], ")"}]}]}], 
          ")"}]}], "12500000000000000"]}], 
      RowBox[{"-", 
       FractionBox[
        RowBox[{"32639", " ", 
         RowBox[{"(", 
          RowBox[{
           RowBox[{
            RowBox[{"(", 
             RowBox[{
              RowBox[{
               RowBox[{"-", "9657960"}], " ", 
               RowBox[{"cos", "(", 
                RowBox[{
                 SubscriptBox["\[Theta]", "3"], "(", "t", ")"}], ")"}]}], "+", 
              RowBox[{"606150", " ", 
               RowBox[{"cos", "(", 
                RowBox[{
                 RowBox[{
                  SubscriptBox["\[Theta]", "3"], "(", "t", ")"}], "-", 
                 RowBox[{
                  SubscriptBox["\[Theta]", "4"], "(", "t", ")"}]}], ")"}]}], 
              "-", 
              RowBox[{"401207755", " ", 
               RowBox[{"sin", "(", 
                RowBox[{
                 SubscriptBox["\[Theta]", "3"], "(", "t", ")"}], ")"}]}], "+", 
              RowBox[{"6820902539", " ", 
               RowBox[{"sin", "(", 
                RowBox[{
                 RowBox[{
                  SubscriptBox["\[Theta]", "3"], "(", "t", ")"}], "-", 
                 RowBox[{
                  SubscriptBox["\[Theta]", "4"], "(", "t", ")"}]}], ")"}]}], 
              "+", 
              RowBox[{"701338095", " ", 
               RowBox[{"sin", "(", 
                RowBox[{
                 RowBox[{
                  SubscriptBox["\[Theta]", "3"], "(", "t", ")"}], "-", 
                 RowBox[{
                  SubscriptBox["\[Theta]", "4"], "(", "t", ")"}], "-", 
                 RowBox[{
                  SubscriptBox["\[Theta]", "5"], "(", "t", ")"}]}], ")"}]}]}],
              ")"}], " ", 
            RowBox[{
             SuperscriptBox[
              SubscriptBox["\[Theta]", "1"], "\[Prime]",
              MultilineFunction->None], "(", "t", ")"}]}], "+", 
           RowBox[{
            RowBox[{"(", 
             RowBox[{
              RowBox[{
               RowBox[{"-", "9657960"}], " ", 
               RowBox[{"cos", "(", 
                RowBox[{
                 SubscriptBox["\[Theta]", "3"], "(", "t", ")"}], ")"}]}], "+", 
              RowBox[{"606150", " ", 
               RowBox[{"cos", "(", 
                RowBox[{
                 RowBox[{
                  SubscriptBox["\[Theta]", "3"], "(", "t", ")"}], "-", 
                 RowBox[{
                  SubscriptBox["\[Theta]", "4"], "(", "t", ")"}]}], ")"}]}], 
              "-", 
              RowBox[{"401207755", " ", 
               RowBox[{"sin", "(", 
                RowBox[{
                 SubscriptBox["\[Theta]", "3"], "(", "t", ")"}], ")"}]}], "+", 
              RowBox[{"6820902539", " ", 
               RowBox[{"sin", "(", 
                RowBox[{
                 RowBox[{
                  SubscriptBox["\[Theta]", "3"], "(", "t", ")"}], "-", 
                 RowBox[{
                  SubscriptBox["\[Theta]", "4"], "(", "t", ")"}]}], ")"}]}], 
              "+", 
              RowBox[{"701338095", " ", 
               RowBox[{"sin", "(", 
                RowBox[{
                 RowBox[{
                  SubscriptBox["\[Theta]", "3"], "(", "t", ")"}], "-", 
                 RowBox[{
                  SubscriptBox["\[Theta]", "4"], "(", "t", ")"}], "-", 
                 RowBox[{
                  SubscriptBox["\[Theta]", "5"], "(", "t", ")"}]}], ")"}]}]}],
              ")"}], " ", 
            RowBox[{
             SuperscriptBox[
              SubscriptBox["\[Theta]", "2"], "\[Prime]",
              MultilineFunction->None], "(", "t", ")"}]}], "+", 
           RowBox[{"701338095", " ", 
            RowBox[{"sin", "(", 
             RowBox[{
              SubscriptBox["\[Theta]", "5"], "(", "t", ")"}], ")"}], " ", 
            RowBox[{
             SuperscriptBox[
              SubscriptBox["\[Theta]", "5"], "\[Prime]",
              MultilineFunction->None], "(", "t", ")"}]}]}], ")"}]}], 
        "6250000000000000"]}], 
      RowBox[{"-", 
       FractionBox[
        RowBox[{"4578194816541", " ", 
         RowBox[{"sin", "(", 
          RowBox[{
           SubscriptBox["\[Theta]", "5"], "(", "t", ")"}], ")"}], " ", 
         RowBox[{
          SuperscriptBox[
           SubscriptBox["\[Theta]", "5"], "\[Prime]",
           MultilineFunction->None], "(", "t", ")"}]}], "1250000000000000"]}], 
      FractionBox[
       RowBox[{"4578194816541", " ", 
        RowBox[{"sin", "(", 
         RowBox[{
          SubscriptBox["\[Theta]", "5"], "(", "t", ")"}], ")"}], " ", 
        RowBox[{
         SuperscriptBox[
          SubscriptBox["\[Theta]", "5"], "\[Prime]",
          MultilineFunction->None], "(", "t", ")"}]}], "1250000000000000"], 
      RowBox[{"-", 
       FractionBox[
        RowBox[{"4578194816541", " ", 
         RowBox[{"sin", "(", 
          RowBox[{
           SubscriptBox["\[Theta]", "5"], "(", "t", ")"}], ")"}], " ", 
         RowBox[{"(", 
          RowBox[{
           RowBox[{
            SuperscriptBox[
             SubscriptBox["\[Theta]", "1"], "\[Prime]",
             MultilineFunction->None], "(", "t", ")"}], "+", 
           RowBox[{
            SuperscriptBox[
             SubscriptBox["\[Theta]", "2"], "\[Prime]",
             MultilineFunction->None], "(", "t", ")"}], "+", 
           RowBox[{
            SuperscriptBox[
             SubscriptBox["\[Theta]", "3"], "\[Prime]",
             MultilineFunction->None], "(", "t", ")"}], "-", 
           RowBox[{
            SuperscriptBox[
             SubscriptBox["\[Theta]", "4"], "\[Prime]",
             MultilineFunction->None], "(", "t", ")"}], "-", 
           RowBox[{
            SuperscriptBox[
             SubscriptBox["\[Theta]", "5"], "\[Prime]",
             MultilineFunction->None], "(", "t", ")"}]}], ")"}]}], 
        "1250000000000000"]}]},
     {
      FractionBox[
       RowBox[{"127", " ", 
        RowBox[{"(", 
         RowBox[{
          RowBox[{
           RowBox[{"(", 
            RowBox[{
             RowBox[{"311561100", " ", 
              RowBox[{"cos", "(", 
               RowBox[{
                RowBox[{
                 SubscriptBox["\[Theta]", "3"], "(", "t", ")"}], "-", 
                RowBox[{
                 SubscriptBox["\[Theta]", "4"], "(", "t", ")"}]}], ")"}]}], 
             "+", 
             RowBox[{"415212750", " ", 
              RowBox[{"cos", "(", 
               RowBox[{
                RowBox[{
                 SubscriptBox["\[Theta]", "2"], "(", "t", ")"}], "+", 
                RowBox[{
                 SubscriptBox["\[Theta]", "3"], "(", "t", ")"}], "-", 
                RowBox[{
                 SubscriptBox["\[Theta]", "4"], "(", "t", ")"}]}], ")"}]}], 
             "+", 
             RowBox[{"3505943905046", " ", 
              RowBox[{"sin", "(", 
               RowBox[{
                RowBox[{
                 SubscriptBox["\[Theta]", "3"], "(", "t", ")"}], "-", 
                RowBox[{
                 SubscriptBox["\[Theta]", "4"], "(", "t", ")"}]}], ")"}]}], 
             "+", 
             RowBox[{"4672318239215", " ", 
              RowBox[{"sin", "(", 
               RowBox[{
                RowBox[{
                 SubscriptBox["\[Theta]", "2"], "(", "t", ")"}], "+", 
                RowBox[{
                 SubscriptBox["\[Theta]", "3"], "(", "t", ")"}], "-", 
                RowBox[{
                 SubscriptBox["\[Theta]", "4"], "(", "t", ")"}]}], ")"}]}], 
             "+", 
             RowBox[{"360487780830", " ", 
              RowBox[{"sin", "(", 
               RowBox[{
                RowBox[{
                 SubscriptBox["\[Theta]", "3"], "(", "t", ")"}], "-", 
                RowBox[{
                 SubscriptBox["\[Theta]", "4"], "(", "t", ")"}], "-", 
                RowBox[{
                 SubscriptBox["\[Theta]", "5"], "(", "t", ")"}]}], ")"}]}], 
             "+", 
             RowBox[{"480416595075", " ", 
              RowBox[{"sin", "(", 
               RowBox[{
                RowBox[{
                 SubscriptBox["\[Theta]", "2"], "(", "t", ")"}], "+", 
                RowBox[{
                 SubscriptBox["\[Theta]", "3"], "(", "t", ")"}], "-", 
                RowBox[{
                 SubscriptBox["\[Theta]", "4"], "(", "t", ")"}], "-", 
                RowBox[{
                 SubscriptBox["\[Theta]", "5"], "(", "t", ")"}]}], ")"}]}]}], 
            ")"}], " ", 
           RowBox[{
            SuperscriptBox[
             SubscriptBox["\[Theta]", "1"], "\[Prime]",
             MultilineFunction->None], "(", "t", ")"}]}], "+", 
          RowBox[{"514", " ", 
           RowBox[{"(", 
            RowBox[{
             RowBox[{
              RowBox[{"(", 
               RowBox[{
                RowBox[{"606150", " ", 
                 RowBox[{"cos", "(", 
                  RowBox[{
                   RowBox[{
                    SubscriptBox["\[Theta]", "3"], "(", "t", ")"}], "-", 
                   RowBox[{
                    SubscriptBox["\[Theta]", "4"], "(", "t", ")"}]}], ")"}]}],
                 "+", 
                RowBox[{"6820902539", " ", 
                 RowBox[{"sin", "(", 
                  RowBox[{
                   RowBox[{
                    SubscriptBox["\[Theta]", "3"], "(", "t", ")"}], "-", 
                   RowBox[{
                    SubscriptBox["\[Theta]", "4"], "(", "t", ")"}]}], ")"}]}],
                 "+", 
                RowBox[{"701338095", " ", 
                 RowBox[{"sin", "(", 
                  RowBox[{
                   RowBox[{
                    SubscriptBox["\[Theta]", "3"], "(", "t", ")"}], "-", 
                   RowBox[{
                    SubscriptBox["\[Theta]", "4"], "(", "t", ")"}], "-", 
                   RowBox[{
                    SubscriptBox["\[Theta]", "5"], "(", "t", ")"}]}], 
                  ")"}]}]}], ")"}], " ", 
              RowBox[{
               SuperscriptBox[
                SubscriptBox["\[Theta]", "2"], "\[Prime]",
                MultilineFunction->None], "(", "t", ")"}]}], "+", 
             RowBox[{"701338095", " ", 
              RowBox[{"sin", "(", 
               RowBox[{
                SubscriptBox["\[Theta]", "5"], "(", "t", ")"}], ")"}], " ", 
              RowBox[{
               SuperscriptBox[
                SubscriptBox["\[Theta]", "5"], "\[Prime]",
                MultilineFunction->None], "(", "t", ")"}]}]}], ")"}]}]}], 
         ")"}]}], "12500000000000000"], 
      FractionBox[
       RowBox[{"32639", " ", 
        RowBox[{"(", 
         RowBox[{
          RowBox[{
           RowBox[{"(", 
            RowBox[{
             RowBox[{"606150", " ", 
              RowBox[{"cos", "(", 
               RowBox[{
                RowBox[{
                 SubscriptBox["\[Theta]", "3"], "(", "t", ")"}], "-", 
                RowBox[{
                 SubscriptBox["\[Theta]", "4"], "(", "t", ")"}]}], ")"}]}], 
             "+", 
             RowBox[{"6820902539", " ", 
              RowBox[{"sin", "(", 
               RowBox[{
                RowBox[{
                 SubscriptBox["\[Theta]", "3"], "(", "t", ")"}], "-", 
                RowBox[{
                 SubscriptBox["\[Theta]", "4"], "(", "t", ")"}]}], ")"}]}], 
             "+", 
             RowBox[{"701338095", " ", 
              RowBox[{"sin", "(", 
               RowBox[{
                RowBox[{
                 SubscriptBox["\[Theta]", "3"], "(", "t", ")"}], "-", 
                RowBox[{
                 SubscriptBox["\[Theta]", "4"], "(", "t", ")"}], "-", 
                RowBox[{
                 SubscriptBox["\[Theta]", "5"], "(", "t", ")"}]}], ")"}]}]}], 
            ")"}], " ", 
           RowBox[{
            SuperscriptBox[
             SubscriptBox["\[Theta]", "1"], "\[Prime]",
             MultilineFunction->None], "(", "t", ")"}]}], "+", 
          RowBox[{
           RowBox[{"(", 
            RowBox[{
             RowBox[{"606150", " ", 
              RowBox[{"cos", "(", 
               RowBox[{
                RowBox[{
                 SubscriptBox["\[Theta]", "3"], "(", "t", ")"}], "-", 
                RowBox[{
                 SubscriptBox["\[Theta]", "4"], "(", "t", ")"}]}], ")"}]}], 
             "+", 
             RowBox[{"6820902539", " ", 
              RowBox[{"sin", "(", 
               RowBox[{
                RowBox[{
                 SubscriptBox["\[Theta]", "3"], "(", "t", ")"}], "-", 
                RowBox[{
                 SubscriptBox["\[Theta]", "4"], "(", "t", ")"}]}], ")"}]}], 
             "+", 
             RowBox[{"701338095", " ", 
              RowBox[{"sin", "(", 
               RowBox[{
                RowBox[{
                 SubscriptBox["\[Theta]", "3"], "(", "t", ")"}], "-", 
                RowBox[{
                 SubscriptBox["\[Theta]", "4"], "(", "t", ")"}], "-", 
                RowBox[{
                 SubscriptBox["\[Theta]", "5"], "(", "t", ")"}]}], ")"}]}]}], 
            ")"}], " ", 
           RowBox[{
            SuperscriptBox[
             SubscriptBox["\[Theta]", "2"], "\[Prime]",
             MultilineFunction->None], "(", "t", ")"}]}], "+", 
          RowBox[{"701338095", " ", 
           RowBox[{"sin", "(", 
            RowBox[{
             SubscriptBox["\[Theta]", "5"], "(", "t", ")"}], ")"}], " ", 
           RowBox[{
            SuperscriptBox[
             SubscriptBox["\[Theta]", "5"], "\[Prime]",
             MultilineFunction->None], "(", "t", ")"}]}]}], ")"}]}], 
       "6250000000000000"], 
      FractionBox[
       RowBox[{"4578194816541", " ", 
        RowBox[{"sin", "(", 
         RowBox[{
          SubscriptBox["\[Theta]", "5"], "(", "t", ")"}], ")"}], " ", 
        RowBox[{
         SuperscriptBox[
          SubscriptBox["\[Theta]", "5"], "\[Prime]",
          MultilineFunction->None], "(", "t", ")"}]}], "1250000000000000"], 
      RowBox[{"-", 
       FractionBox[
        RowBox[{"4578194816541", " ", 
         RowBox[{"sin", "(", 
          RowBox[{
           SubscriptBox["\[Theta]", "5"], "(", "t", ")"}], ")"}], " ", 
         RowBox[{
          SuperscriptBox[
           SubscriptBox["\[Theta]", "5"], "\[Prime]",
           MultilineFunction->None], "(", "t", ")"}]}], "1250000000000000"]}], 
      FractionBox[
       RowBox[{"4578194816541", " ", 
        RowBox[{"sin", "(", 
         RowBox[{
          SubscriptBox["\[Theta]", "5"], "(", "t", ")"}], ")"}], " ", 
        RowBox[{"(", 
         RowBox[{
          RowBox[{
           SuperscriptBox[
            SubscriptBox["\[Theta]", "1"], "\[Prime]",
            MultilineFunction->None], "(", "t", ")"}], "+", 
          RowBox[{
           SuperscriptBox[
            SubscriptBox["\[Theta]", "2"], "\[Prime]",
            MultilineFunction->None], "(", "t", ")"}], "+", 
          RowBox[{
           SuperscriptBox[
            SubscriptBox["\[Theta]", "3"], "\[Prime]",
            MultilineFunction->None], "(", "t", ")"}], "-", 
          RowBox[{
           SuperscriptBox[
            SubscriptBox["\[Theta]", "4"], "\[Prime]",
            MultilineFunction->None], "(", "t", ")"}], "-", 
          RowBox[{
           SuperscriptBox[
            SubscriptBox["\[Theta]", "5"], "\[Prime]",
            MultilineFunction->None], "(", "t", ")"}]}], ")"}]}], 
       "1250000000000000"]},
     {
      FractionBox[
       RowBox[{"17813987613", " ", 
        RowBox[{"(", 
         RowBox[{
          RowBox[{
           RowBox[{"(", 
            RowBox[{
             RowBox[{"514", " ", 
              RowBox[{"sin", "(", 
               RowBox[{
                RowBox[{
                 SubscriptBox["\[Theta]", "3"], "(", "t", ")"}], "-", 
                RowBox[{
                 SubscriptBox["\[Theta]", "4"], "(", "t", ")"}], "-", 
                RowBox[{
                 SubscriptBox["\[Theta]", "5"], "(", "t", ")"}]}], ")"}]}], 
             "+", 
             RowBox[{"685", " ", 
              RowBox[{"sin", "(", 
               RowBox[{
                RowBox[{
                 SubscriptBox["\[Theta]", "2"], "(", "t", ")"}], "+", 
                RowBox[{
                 SubscriptBox["\[Theta]", "3"], "(", "t", ")"}], "-", 
                RowBox[{
                 SubscriptBox["\[Theta]", "4"], "(", "t", ")"}], "-", 
                RowBox[{
                 SubscriptBox["\[Theta]", "5"], "(", "t", ")"}]}], ")"}]}], 
             "+", 
             RowBox[{"514", " ", 
              RowBox[{"sin", "(", 
               RowBox[{
                SubscriptBox["\[Theta]", "5"], "(", "t", ")"}], ")"}]}]}], 
            ")"}], " ", 
           RowBox[{
            SuperscriptBox[
             SubscriptBox["\[Theta]", "1"], "\[Prime]",
             MultilineFunction->None], "(", "t", ")"}]}], "+", 
          RowBox[{"514", " ", 
           RowBox[{"(", 
            RowBox[{
             RowBox[{
              RowBox[{"(", 
               RowBox[{
                RowBox[{"sin", "(", 
                 RowBox[{
                  RowBox[{
                   SubscriptBox["\[Theta]", "3"], "(", "t", ")"}], "-", 
                  RowBox[{
                   SubscriptBox["\[Theta]", "4"], "(", "t", ")"}], "-", 
                  RowBox[{
                   SubscriptBox["\[Theta]", "5"], "(", "t", ")"}]}], ")"}], 
                "+", 
                RowBox[{"sin", "(", 
                 RowBox[{
                  SubscriptBox["\[Theta]", "5"], "(", "t", ")"}], ")"}]}], 
               ")"}], " ", 
              RowBox[{
               SuperscriptBox[
                SubscriptBox["\[Theta]", "2"], "\[Prime]",
                MultilineFunction->None], "(", "t", ")"}]}], "+", 
             RowBox[{
              RowBox[{"sin", "(", 
               RowBox[{
                SubscriptBox["\[Theta]", "5"], "(", "t", ")"}], ")"}], " ", 
              RowBox[{"(", 
               RowBox[{
                RowBox[{
                 SuperscriptBox[
                  SubscriptBox["\[Theta]", "3"], "\[Prime]",
                  MultilineFunction->None], "(", "t", ")"}], "-", 
                RowBox[{
                 SuperscriptBox[
                  SubscriptBox["\[Theta]", "4"], "\[Prime]",
                  MultilineFunction->None], "(", "t", ")"}]}], ")"}]}]}], 
            ")"}]}]}], ")"}]}], "2500000000000000"], 
      FractionBox[
       RowBox[{"4578194816541", " ", 
        RowBox[{"(", 
         RowBox[{
          RowBox[{
           RowBox[{"(", 
            RowBox[{
             RowBox[{"sin", "(", 
              RowBox[{
               RowBox[{
                SubscriptBox["\[Theta]", "3"], "(", "t", ")"}], "-", 
               RowBox[{
                SubscriptBox["\[Theta]", "4"], "(", "t", ")"}], "-", 
               RowBox[{
                SubscriptBox["\[Theta]", "5"], "(", "t", ")"}]}], ")"}], "+", 
             
             RowBox[{"sin", "(", 
              RowBox[{
               SubscriptBox["\[Theta]", "5"], "(", "t", ")"}], ")"}]}], ")"}],
            " ", 
           RowBox[{
            SuperscriptBox[
             SubscriptBox["\[Theta]", "1"], "\[Prime]",
             MultilineFunction->None], "(", "t", ")"}]}], "+", 
          RowBox[{
           RowBox[{"(", 
            RowBox[{
             RowBox[{"sin", "(", 
              RowBox[{
               RowBox[{
                SubscriptBox["\[Theta]", "3"], "(", "t", ")"}], "-", 
               RowBox[{
                SubscriptBox["\[Theta]", "4"], "(", "t", ")"}], "-", 
               RowBox[{
                SubscriptBox["\[Theta]", "5"], "(", "t", ")"}]}], ")"}], "+", 
             
             RowBox[{"sin", "(", 
              RowBox[{
               SubscriptBox["\[Theta]", "5"], "(", "t", ")"}], ")"}]}], ")"}],
            " ", 
           RowBox[{
            SuperscriptBox[
             SubscriptBox["\[Theta]", "2"], "\[Prime]",
             MultilineFunction->None], "(", "t", ")"}]}], "+", 
          RowBox[{
           RowBox[{"sin", "(", 
            RowBox[{
             SubscriptBox["\[Theta]", "5"], "(", "t", ")"}], ")"}], " ", 
           RowBox[{"(", 
            RowBox[{
             RowBox[{
              SuperscriptBox[
               SubscriptBox["\[Theta]", "3"], "\[Prime]",
               MultilineFunction->None], "(", "t", ")"}], "-", 
             RowBox[{
              SuperscriptBox[
               SubscriptBox["\[Theta]", "4"], "\[Prime]",
               MultilineFunction->None], "(", "t", ")"}]}], ")"}]}]}], 
         ")"}]}], "1250000000000000"], 
      FractionBox[
       RowBox[{"4578194816541", " ", 
        RowBox[{"sin", "(", 
         RowBox[{
          SubscriptBox["\[Theta]", "5"], "(", "t", ")"}], ")"}], " ", 
        RowBox[{"(", 
         RowBox[{
          RowBox[{
           SuperscriptBox[
            SubscriptBox["\[Theta]", "1"], "\[Prime]",
            MultilineFunction->None], "(", "t", ")"}], "+", 
          RowBox[{
           SuperscriptBox[
            SubscriptBox["\[Theta]", "2"], "\[Prime]",
            MultilineFunction->None], "(", "t", ")"}], "+", 
          RowBox[{
           SuperscriptBox[
            SubscriptBox["\[Theta]", "3"], "\[Prime]",
            MultilineFunction->None], "(", "t", ")"}], "-", 
          RowBox[{
           SuperscriptBox[
            SubscriptBox["\[Theta]", "4"], "\[Prime]",
            MultilineFunction->None], "(", "t", ")"}]}], ")"}]}], 
       "1250000000000000"], 
      RowBox[{"-", 
       FractionBox[
        RowBox[{"4578194816541", " ", 
         RowBox[{"sin", "(", 
          RowBox[{
           SubscriptBox["\[Theta]", "5"], "(", "t", ")"}], ")"}], " ", 
         RowBox[{"(", 
          RowBox[{
           RowBox[{
            SuperscriptBox[
             SubscriptBox["\[Theta]", "1"], "\[Prime]",
             MultilineFunction->None], "(", "t", ")"}], "+", 
           RowBox[{
            SuperscriptBox[
             SubscriptBox["\[Theta]", "2"], "\[Prime]",
             MultilineFunction->None], "(", "t", ")"}], "+", 
           RowBox[{
            SuperscriptBox[
             SubscriptBox["\[Theta]", "3"], "\[Prime]",
             MultilineFunction->None], "(", "t", ")"}], "-", 
           RowBox[{
            SuperscriptBox[
             SubscriptBox["\[Theta]", "4"], "\[Prime]",
             MultilineFunction->None], "(", "t", ")"}]}], ")"}]}], 
        "1250000000000000"]}], "0"}
    },
    GridBoxAlignment->{
     "Columns" -> {{Center}}, "ColumnsIndexed" -> {}, "Rows" -> {{Baseline}}, 
      "RowsIndexed" -> {}},
    GridBoxSpacings->{"Columns" -> {
        Offset[0.27999999999999997`], {
         Offset[0.7]}, 
        Offset[0.27999999999999997`]}, "ColumnsIndexed" -> {}, "Rows" -> {
        Offset[0.2], {
         Offset[0.4]}, 
        Offset[0.2]}, "RowsIndexed" -> {}}], "\[NoBreak]", ")"}], 
  TraditionalForm]], "Output",
 CellChangeTimes->{
  3.527281441454191*^9, {3.527281631674336*^9, 3.5272816441646433`*^9}, 
   3.527281945281845*^9}]
}, Open  ]],

Cell[CellGroupData[{

Cell[BoxData[{
 RowBox[{
  RowBox[{"h", "=", 
   RowBox[{"Simplify", "[", 
    RowBox[{
     RowBox[{
      RowBox[{
       SubscriptBox["g", "nsf"], "[", "\[Theta]", "]"}], 
      "\[LeftDoubleBracket]", 
      RowBox[{"3", ",", "4"}], "\[RightDoubleBracket]"}], "/.", 
     "\[InvisibleSpace]", "p0"}], "]"}]}], ";"}], "\[IndentingNewLine]", 
 RowBox[{
  RowBox[{
   RowBox[{"hdot", " ", "=", "  ", 
    RowBox[{"Simplify", "[", 
     RowBox[{
      SubscriptBox["\[PartialD]", 
       RowBox[{"{", 
        RowBox[{
         RowBox[{"Flatten", "[", "q", "]"}], ",", "1"}], "}"}]], 
      RowBox[{"Flatten", "[", "h", "]"}]}], "]"}]}], ";"}], 
  "\[IndentingNewLine]", 
  RowBox[{"(*", 
   RowBox[{"hip", " ", "position", " ", "related"}], "*)"}], "\n", 
  RowBox[{"(*", 
   RowBox[{"1", " ", "Hip", " ", "Position"}], 
   "*)"}]}], "\[IndentingNewLine]", 
 RowBox[{
  RowBox[{
   RowBox[{"HipPos", " ", "=", " ", 
    RowBox[{
     RowBox[{
      RowBox[{
       SubscriptBox["g", "hip"], "[", "\[Theta]", "]"}], 
      "\[LeftDoubleBracket]", 
      RowBox[{"1", ",", "4"}], "\[RightDoubleBracket]"}], "/.", "p0"}]}], 
   ";"}], "\n", 
  RowBox[{"(*", 
   RowBox[{"2", " ", "Linearized", " ", "Hip", " ", "Position"}], 
   "*)"}]}], "\n", 
 RowBox[{
  RowBox[{
   RowBox[{"LHipPos", " ", "=", " ", 
    RowBox[{"Linearize", "[", 
     RowBox[{"HipPos", ",", 
      RowBox[{"Table", "[", 
       RowBox[{
        RowBox[{
         RowBox[{"Subscript", "[", 
          RowBox[{"\[Theta]", ",", "i"}], "]"}], "[", "t", "]"}], ",", 
        RowBox[{"{", 
         RowBox[{"i", ",", "5"}], "}"}]}], "]"}], ",", 
      RowBox[{"Table", "[", 
       RowBox[{"0", ",", 
        RowBox[{"{", 
         RowBox[{"i", ",", "5"}], "}"}]}], "]"}]}], "]"}]}], ";"}], 
  "\[IndentingNewLine]", 
  RowBox[{"(*", 
   RowBox[{"non", "-", 
    RowBox[{"stance", " ", "slope", " ", "related"}]}], "*)"}], "\n", 
  RowBox[{"(*", 
   RowBox[{
    RowBox[{"1", " ", "non"}], "-", 
    RowBox[{"stance", " ", "slope"}]}], "*)"}]}], "\[IndentingNewLine]", 
 RowBox[{
  RowBox[{"nsslope", " ", "=", " ", 
   RowBox[{
    FractionBox[
     RowBox[{
      RowBox[{
       RowBox[{
        SubscriptBox["g", "nsf"], "[", "\[Theta]", "]"}], 
       "\[LeftDoubleBracket]", 
       RowBox[{"1", ",", "4"}], "\[RightDoubleBracket]"}], "-", 
      RowBox[{
       RowBox[{
        SubscriptBox["g", "hip"], "[", "\[Theta]", "]"}], 
       "\[LeftDoubleBracket]", 
       RowBox[{"1", ",", "4"}], "\[RightDoubleBracket]"}]}], 
     RowBox[{
      RowBox[{
       RowBox[{
        SubscriptBox["g", "nsf"], "[", "\[Theta]", "]"}], 
       "\[LeftDoubleBracket]", 
       RowBox[{"3", ",", "4"}], "\[RightDoubleBracket]"}], "-", 
      RowBox[{
       RowBox[{
        SubscriptBox["g", "hip"], "[", "\[Theta]", "]"}], 
       "\[LeftDoubleBracket]", 
       RowBox[{"3", ",", "4"}], "\[RightDoubleBracket]"}]}]], "/.", "p0"}]}], 
  "\[IndentingNewLine]", 
  RowBox[{"(*", 
   RowBox[{
    RowBox[{"2.", " ", "linearized", " ", "non"}], "-", 
    RowBox[{"stance", " ", "slope"}]}], "*)"}]}], "\[IndentingNewLine]", 
 RowBox[{
  RowBox[{
   RowBox[{"LinearNSslope", "=", 
    RowBox[{"Linearize", "[", 
     RowBox[{
      RowBox[{"nsslope", " ", "/.", "p0"}], ",", 
      RowBox[{"Table", "[", 
       RowBox[{
        RowBox[{
         RowBox[{"Subscript", "[", 
          RowBox[{"\[Theta]", ",", "i"}], "]"}], "[", "t", "]"}], ",", 
        RowBox[{"{", 
         RowBox[{"i", ",", "5"}], "}"}]}], "]"}], ",", 
      RowBox[{"Table", "[", 
       RowBox[{"0", ",", 
        RowBox[{"{", 
         RowBox[{"i", ",", "5"}], "}"}]}], "]"}]}], "]"}]}], ";"}], 
  "\[IndentingNewLine]", 
  RowBox[{"(*", 
   RowBox[{"3.", " ", "hip", " ", "angle"}], "*)"}]}], "\n", 
 RowBox[{
  RowBox[{
   RowBox[{"HipAngle", " ", "=", " ", 
    RowBox[{
     RowBox[{
      SubscriptBox["\[Theta]", "3"], "[", "t", "]"}], "-", 
     RowBox[{
      SubscriptBox["\[Theta]", "4"], "[", "t", "]"}]}]}], ";"}], 
  "\[IndentingNewLine]", 
  RowBox[{"(*", 
   RowBox[{"torso", " ", "related"}], "*)"}], "\[IndentingNewLine]", 
  RowBox[{"(*", 
   RowBox[{
    RowBox[{"1", " ", "Torso", " ", "non"}], "-", 
    RowBox[{"stance", " ", "thigh", " ", "angle"}]}], 
   "*)"}]}], "\[IndentingNewLine]", 
 RowBox[{
  RowBox[{
   RowBox[{"theta4", " ", "=", " ", 
    RowBox[{
     SubscriptBox["\[Theta]", "4"], "[", "t", "]"}]}], ";"}], "\n", 
  RowBox[{"(*", 
   RowBox[{"2", " ", "Torso", " ", "Hip", " ", "angle"}], 
   "*)"}]}], "\[IndentingNewLine]", 
 RowBox[{
  RowBox[{
   RowBox[{"TorsoHipAngle", "=", " ", 
    RowBox[{
     RowBox[{
      SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
     RowBox[{
      SubscriptBox["\[Theta]", "2"], "[", "t", "]"}], "+", 
     RowBox[{
      SubscriptBox["\[Theta]", "3"], "[", "t", "]"}]}]}], ";"}], "\n", 
  RowBox[{"(*", 
   RowBox[{
    RowBox[{"3", " ", "Torso", " ", "Non"}], "-", 
    RowBox[{"stance", " ", "Slope"}]}], "*)"}]}], "\[IndentingNewLine]", 
 RowBox[{
  RowBox[{
   RowBox[{"nstorso", " ", "=", " ", 
    FractionBox[
     RowBox[{
      RowBox[{
       RowBox[{
        SubscriptBox["g", "nsf"], "[", "\[Theta]", "]"}], 
       "\[LeftDoubleBracket]", 
       RowBox[{"1", ",", "4"}], "\[RightDoubleBracket]"}], "-", 
      RowBox[{
       RowBox[{
        SubscriptBox["g", "torso"], "[", "\[Theta]", "]"}], 
       "\[LeftDoubleBracket]", 
       RowBox[{"1", ",", "4"}], "\[RightDoubleBracket]"}]}], 
     RowBox[{
      RowBox[{
       RowBox[{
        SubscriptBox["g", "nsf"], "[", "\[Theta]", "]"}], 
       "\[LeftDoubleBracket]", 
       RowBox[{"3", ",", "4"}], "\[RightDoubleBracket]"}], "-", 
      RowBox[{
       RowBox[{
        SubscriptBox["g", "torso"], "[", "\[Theta]", "]"}], 
       "\[LeftDoubleBracket]", 
       RowBox[{"3", ",", "4"}], "\[RightDoubleBracket]"}]}]]}], ";"}], "\n", 
  RowBox[{"(*", 
   RowBox[{
    RowBox[{"4", " ", "Linearized", " ", "torso", " ", "non"}], "-", 
    RowBox[{"stance", " ", "slope", " ", "angle"}]}], 
   "*)"}]}], "\[IndentingNewLine]", 
 RowBox[{
  RowBox[{
   RowBox[{"LinearNStorso", "=", 
    RowBox[{"Linearize", "[", 
     RowBox[{
      RowBox[{"nstorso", " ", "/.", "p0"}], ",", 
      RowBox[{"Table", "[", 
       RowBox[{
        RowBox[{
         RowBox[{"Subscript", "[", 
          RowBox[{"\[Theta]", ",", "i"}], "]"}], "[", "t", "]"}], ",", 
        RowBox[{"{", 
         RowBox[{"i", ",", "5"}], "}"}]}], "]"}], ",", 
      RowBox[{"Table", "[", 
       RowBox[{"0", ",", 
        RowBox[{"{", 
         RowBox[{"i", ",", "5"}], "}"}]}], "]"}]}], "]"}]}], ";"}], "\n", 
  RowBox[{"(*", 
   RowBox[{"5", " ", "Torso", " ", "stance", " ", "slope"}], 
   "*)"}]}], "\[IndentingNewLine]", 
 RowBox[{
  RowBox[{
   RowBox[{"storso", " ", "=", " ", 
    FractionBox[
     RowBox[{
      RowBox[{
       SubscriptBox["g", "torso"], "[", "\[Theta]", "]"}], 
      "\[LeftDoubleBracket]", 
      RowBox[{"1", ",", "4"}], "\[RightDoubleBracket]"}], 
     RowBox[{
      RowBox[{
       SubscriptBox["g", "torso"], "[", "\[Theta]", "]"}], 
      "\[LeftDoubleBracket]", 
      RowBox[{"3", ",", "4"}], "\[RightDoubleBracket]"}]]}], ";"}], "\n", 
  RowBox[{"(*", 
   RowBox[{
   "6", " ", "Linearized", " ", "Torso", " ", "stance", " ", "slope"}], 
   "*)"}]}], "\n", 
 RowBox[{
  RowBox[{
   RowBox[{"LinearStorso", "=", 
    RowBox[{"Linearize", "[", 
     RowBox[{
      RowBox[{"storso", " ", "/.", "p0"}], ",", 
      RowBox[{"Table", "[", 
       RowBox[{
        RowBox[{
         RowBox[{"Subscript", "[", 
          RowBox[{"\[Theta]", ",", "i"}], "]"}], "[", "t", "]"}], ",", 
        RowBox[{"{", 
         RowBox[{"i", ",", "5"}], "}"}]}], "]"}], ",", 
      RowBox[{"Table", "[", 
       RowBox[{"0", ",", 
        RowBox[{"{", 
         RowBox[{"i", ",", "5"}], "}"}]}], "]"}]}], "]"}]}], ";"}], 
  "\[IndentingNewLine]", 
  RowBox[{"(*", 
   RowBox[{"7", " ", "COM"}], "*)"}]}], "\[IndentingNewLine]", 
 RowBox[{
  RowBox[{
   SubscriptBox["p", "COM"], "=", 
   RowBox[{
    RowBox[{
     RowBox[{"(", 
      RowBox[{
       RowBox[{
        UnderoverscriptBox["\[Sum]", 
         RowBox[{"i", "=", "1"}], 
         RowBox[{"Length", "[", "mm", "]"}]], 
        RowBox[{
         RowBox[{"mm", "\[LeftDoubleBracket]", "i", "\[RightDoubleBracket]"}], 
         RowBox[{
          RowBox[{
           RowBox[{
            SubscriptBox["g", "i"], "[", "\[Theta]", "]"}], 
           "\[LeftDoubleBracket]", 
           RowBox[{"1", ",", "4"}], "\[RightDoubleBracket]"}], "/", 
          RowBox[{"(", 
           RowBox[{
            RowBox[{"2", "*", 
             RowBox[{"(", 
              RowBox[{"mc", "+", "mt"}], ")"}]}], "+", "mh"}], ")"}]}]}]}], "/.",
        "constsubs"}], ")"}], "//.", "p0"}], "//", "Simplify"}]}], 
  ";"}], "\n", 
 RowBox[{
  RowBox[{"(*", 
   RowBox[{"8", " ", "Linearized", " ", "COM"}], "*)"}], 
  RowBox[{
   RowBox[{
    SubscriptBox["Linearp", "COM"], "=", " ", 
    RowBox[{"Linearize", "[", 
     RowBox[{
      RowBox[{
       SubscriptBox["p", "COM"], "/.", "p0"}], ",", 
      RowBox[{"Table", "[", 
       RowBox[{
        RowBox[{
         RowBox[{"Subscript", "[", 
          RowBox[{"\[Theta]", ",", "i"}], "]"}], "[", "t", "]"}], ",", 
        RowBox[{"{", 
         RowBox[{"i", ",", "5"}], "}"}]}], "]"}], ",", 
      RowBox[{"Table", "[", 
       RowBox[{"0", ",", 
        RowBox[{"{", 
         RowBox[{"i", ",", "5"}], "}"}]}], "]"}]}], "]"}]}], 
   ";"}]}]}], "Input",
 CellChangeTimes->{
  3.504875918224949*^9, {3.505063952502133*^9, 3.505063953970308*^9}, {
   3.5051489336912956`*^9, 3.5051489340032964`*^9}, 3.513531757509935*^9, {
   3.515870024222504*^9, 3.5158700252255616`*^9}, 3.5158863755610266`*^9, {
   3.5271753048660097`*^9, 3.527175316328361*^9}, {3.5271784037700644`*^9, 
   3.527178409550562*^9}, {3.527194806620797*^9, 3.527194812727977*^9}, {
   3.527194901003693*^9, 3.527194939760748*^9}, 3.527195000990027*^9, 
   3.527195065536849*^9, {3.527197843268285*^9, 3.527197851612855*^9}, {
   3.527266876626479*^9, 3.5272669091021013`*^9}, 3.5272680582479362`*^9, {
   3.52726826837751*^9, 3.5272682685913153`*^9}, {3.527268323380795*^9, 
   3.52726834320557*^9}, {3.527268721025804*^9, 3.527268740044486*^9}, {
   3.527281326545467*^9, 3.527281336017424*^9}, 3.527282982629828*^9}],

Cell[BoxData[
 FormBox[
  FractionBox[
   RowBox[{
    FractionBox[
     RowBox[{"127", " ", 
      RowBox[{"(", 
       RowBox[{
        RowBox[{"685", " ", 
         RowBox[{"sin", "(", 
          RowBox[{
           SubscriptBox["\[Theta]", "1"], "(", "t", ")"}], ")"}]}], "+", 
        RowBox[{"514", " ", 
         RowBox[{"sin", "(", 
          RowBox[{
           RowBox[{
            SubscriptBox["\[Theta]", "1"], "(", "t", ")"}], "+", 
           RowBox[{
            SubscriptBox["\[Theta]", "2"], "(", "t", ")"}]}], ")"}]}]}], 
       ")"}]}], "250000"], "+", 
    FractionBox[
     RowBox[{"127", " ", 
      RowBox[{"(", 
       RowBox[{
        RowBox[{"-", 
         RowBox[{"685", " ", 
          RowBox[{"sin", "(", 
           RowBox[{
            SubscriptBox["\[Theta]", "1"], "(", "t", ")"}], ")"}]}]}], "-", 
        RowBox[{"514", " ", 
         RowBox[{"sin", "(", 
          RowBox[{
           RowBox[{
            SubscriptBox["\[Theta]", "1"], "(", "t", ")"}], "+", 
           RowBox[{
            SubscriptBox["\[Theta]", "2"], "(", "t", ")"}]}], ")"}]}], "+", 
        RowBox[{"514", " ", 
         RowBox[{"sin", "(", 
          RowBox[{
           RowBox[{
            SubscriptBox["\[Theta]", "1"], "(", "t", ")"}], "+", 
           RowBox[{
            SubscriptBox["\[Theta]", "2"], "(", "t", ")"}], "+", 
           RowBox[{
            SubscriptBox["\[Theta]", "3"], "(", "t", ")"}], "-", 
           RowBox[{
            SubscriptBox["\[Theta]", "4"], "(", "t", ")"}]}], ")"}]}], "+", 
        RowBox[{"685", " ", 
         RowBox[{"sin", "(", 
          RowBox[{
           RowBox[{
            SubscriptBox["\[Theta]", "1"], "(", "t", ")"}], "+", 
           RowBox[{
            SubscriptBox["\[Theta]", "2"], "(", "t", ")"}], "+", 
           RowBox[{
            SubscriptBox["\[Theta]", "3"], "(", "t", ")"}], "-", 
           RowBox[{
            SubscriptBox["\[Theta]", "4"], "(", "t", ")"}], "-", 
           RowBox[{
            SubscriptBox["\[Theta]", "5"], "(", "t", ")"}]}], ")"}]}]}], 
       ")"}]}], "250000"]}], 
   RowBox[{
    FractionBox[
     RowBox[{"127", " ", 
      RowBox[{"(", 
       RowBox[{
        RowBox[{"685", " ", 
         RowBox[{"cos", "(", 
          RowBox[{
           SubscriptBox["\[Theta]", "1"], "(", "t", ")"}], ")"}]}], "+", 
        RowBox[{"514", " ", 
         RowBox[{"cos", "(", 
          RowBox[{
           RowBox[{
            SubscriptBox["\[Theta]", "1"], "(", "t", ")"}], "+", 
           RowBox[{
            SubscriptBox["\[Theta]", "2"], "(", "t", ")"}]}], ")"}]}], "-", 
        RowBox[{"514", " ", 
         RowBox[{"cos", "(", 
          RowBox[{
           RowBox[{
            SubscriptBox["\[Theta]", "1"], "(", "t", ")"}], "+", 
           RowBox[{
            SubscriptBox["\[Theta]", "2"], "(", "t", ")"}], "+", 
           RowBox[{
            SubscriptBox["\[Theta]", "3"], "(", "t", ")"}], "-", 
           RowBox[{
            SubscriptBox["\[Theta]", "4"], "(", "t", ")"}]}], ")"}]}], "-", 
        RowBox[{"685", " ", 
         RowBox[{"cos", "(", 
          RowBox[{
           RowBox[{
            SubscriptBox["\[Theta]", "1"], "(", "t", ")"}], "+", 
           RowBox[{
            SubscriptBox["\[Theta]", "2"], "(", "t", ")"}], "+", 
           RowBox[{
            SubscriptBox["\[Theta]", "3"], "(", "t", ")"}], "-", 
           RowBox[{
            SubscriptBox["\[Theta]", "4"], "(", "t", ")"}], "-", 
           RowBox[{
            SubscriptBox["\[Theta]", "5"], "(", "t", ")"}]}], ")"}]}]}], 
       ")"}]}], "250000"], "-", 
    FractionBox[
     RowBox[{"127", " ", 
      RowBox[{"(", 
       RowBox[{
        RowBox[{"685", " ", 
         RowBox[{"cos", "(", 
          RowBox[{
           SubscriptBox["\[Theta]", "1"], "(", "t", ")"}], ")"}]}], "+", 
        RowBox[{"514", " ", 
         RowBox[{"cos", "(", 
          RowBox[{
           RowBox[{
            SubscriptBox["\[Theta]", "1"], "(", "t", ")"}], "+", 
           RowBox[{
            SubscriptBox["\[Theta]", "2"], "(", "t", ")"}]}], ")"}]}]}], 
       ")"}]}], "250000"]}]], TraditionalForm]], "Output",
 CellChangeTimes->{3.5272829834438*^9}]
}, Open  ]],

Cell[BoxData[""], "Input",
 CellChangeTimes->{
  3.504875918224949*^9, {3.505063952502133*^9, 3.505063953970308*^9}, {
   3.5051489336912956`*^9, 3.5051489340032964`*^9}, 3.513531757509935*^9, {
   3.515870024222504*^9, 3.5158700252255616`*^9}, 3.5158863755610266`*^9, {
   3.5271753048660097`*^9, 3.527175316328361*^9}, {3.5271784037700644`*^9, 
   3.527178409550562*^9}, {3.527194806620797*^9, 3.527194812727977*^9}, {
   3.527194901003693*^9, 3.527194939760748*^9}, 3.527195000990027*^9, 
   3.527195065536849*^9, {3.527197843268285*^9, 3.527197851612855*^9}, {
   3.527266876626479*^9, 3.5272669091021013`*^9}, 3.5272680582479362`*^9, {
   3.52726826837751*^9, 3.5272682685913153`*^9}, {3.527268323380795*^9, 
   3.52726834320557*^9}, 3.527268721025804*^9}],

Cell[BoxData[
 RowBox[{"\n", "\[IndentingNewLine]", 
  RowBox[{"(*", 
   RowBox[{"feedback", " ", "control"}], "*)"}], "\[IndentingNewLine]", 
  RowBox[{
   RowBox[{
    RowBox[{"\[Chi]", "=", 
     RowBox[{"Join", "[", 
      RowBox[{"q", ",", "dq"}], "]"}]}], ";"}], "\[IndentingNewLine]", 
   RowBox[{
    RowBox[{"d\[Chi]", "=", 
     RowBox[{"D", "[", 
      RowBox[{"\[Chi]", ",", "t"}], "]"}]}], ";"}], "\n", 
   RowBox[{
    RowBox[{"hipOutput", " ", "=", " ", 
     RowBox[{"{", 
      RowBox[{"HipPos", ",", "LHipPos"}], "}"}]}], ";"}], 
   "\[IndentingNewLine]", 
   RowBox[{
    RowBox[{
     SubscriptBox["p", "hip"], " ", "=", 
     RowBox[{"hipOutput", "[", 
      RowBox[{"[", "HipPosIndex", "]"}], "]"}]}], " ", ";"}], 
   "\[IndentingNewLine]", 
   RowBox[{
    RowBox[{
     SubscriptBox["p", "hipdot"], "  ", "=", " ", 
     RowBox[{"Simplify", "[", 
      RowBox[{
       SubscriptBox["\[PartialD]", 
        RowBox[{"{", 
         RowBox[{
          RowBox[{"Flatten", "[", "q", "]"}], ",", "1"}], "}"}]], 
       RowBox[{"Flatten", "[", 
        SubscriptBox["p", "hip"], "]"}]}], "]"}]}], ";"}], 
   "\[IndentingNewLine]", 
   RowBox[{
    RowBox[{
     SubscriptBox["v", "hip"], "=", 
     RowBox[{
      RowBox[{"D", "[", 
       RowBox[{
        SubscriptBox["p", "hip"], ",", "t"}], "]"}], "//", "Simplify"}]}], 
    ";"}], "\n", 
   RowBox[{
    RowBox[{"fb", "=", 
     RowBox[{"{", 
      RowBox[{
       RowBox[{"\[Sigma]", "[", "t", "]"}], "\[Rule]", 
       FractionBox[
        RowBox[{
         SubscriptBox["p", "hip"], "-", 
         RowBox[{"p", "[", "1", "]"}]}], 
        RowBox[{"a", "[", 
         RowBox[{"1", ",", "1"}], "]"}]]}], "}"}]}], ";"}], 
   RowBox[{"(*", 
    RowBox[{"time", "-", 
     RowBox[{"invariant", " ", 
      RowBox[{"parameterization", ":"}]}]}], "*)"}], "\n", 
   RowBox[{
    RowBox[{"\[Sigma]y", "=", 
     RowBox[{
      RowBox[{"\[Sigma]", "[", "t", "]"}], "/.", "fb"}]}], ";"}], 
   "\[IndentingNewLine]", 
   RowBox[{
    RowBox[{
     RowBox[{"HumanFunction", "[", "i_", "]"}], ":=", 
     RowBox[{
      FractionBox[
       RowBox[{
        RowBox[{
         RowBox[{"a", "[", 
          RowBox[{"i", ",", "1"}], "]"}], " ", 
         RowBox[{"Cos", "[", 
          RowBox[{
           RowBox[{"a", "[", 
            RowBox[{"i", ",", "2"}], "]"}], " ", 
           RowBox[{"\[Sigma]", "[", "t", "]"}]}], "]"}]}], "+", 
        RowBox[{
         RowBox[{"a", "[", 
          RowBox[{"i", ",", "3"}], "]"}], " ", 
         RowBox[{"Sin", "[", 
          RowBox[{
           RowBox[{"a", "[", 
            RowBox[{"i", ",", "2"}], "]"}], " ", 
           RowBox[{"\[Sigma]", "[", "t", "]"}]}], "]"}]}]}], 
       RowBox[{"Exp", "[", 
        RowBox[{
         RowBox[{"a", "[", 
          RowBox[{"i", ",", "4"}], "]"}], 
         RowBox[{"\[Sigma]", "[", "t", "]"}]}], "]"}]], "+", 
      RowBox[{"a", "[", 
       RowBox[{"i", ",", "5"}], "]"}]}]}], ";"}], 
   RowBox[{"(*", 
    RowBox[{"calculate", 
     FormBox[
      SubscriptBox["y", "d"],
      TraditionalForm], "and", " ", "its", " ", "derivatives"}], "*)"}], "\n",
    "\n", 
   RowBox[{
    RowBox[{
     SubscriptBox["y", 
      RowBox[{"d", ",", "1"}]], " ", "=", " ", 
     RowBox[{"a", "[", 
      RowBox[{"1", ",", "1"}], "]"}]}], ";"}], "\n", 
   RowBox[{
    RowBox[{
     SubscriptBox["y", 
      RowBox[{"d", ",", "2"}]], " ", "=", " ", 
     RowBox[{
      RowBox[{"Transpose", "[", 
       RowBox[{"{", 
        RowBox[{"Table", "[", 
         RowBox[{
          RowBox[{"HumanFunction", "[", "i", "]"}], ",", 
          RowBox[{"{", 
           RowBox[{"i", ",", " ", "2", ",", "ndof"}], "}"}]}], "]"}], "}"}], 
       "]"}], "/.", "fb"}]}], ";"}], "\n", 
   RowBox[{
    RowBox[{
     SubscriptBox["Dy", 
      RowBox[{"d", ",", "1"}]], "=", 
     RowBox[{"Simplify", "[", 
      RowBox[{
       SubscriptBox["\[PartialD]", 
        RowBox[{"{", 
         RowBox[{
          RowBox[{"Flatten", "[", "\[Chi]", "]"}], ",", "1"}], "}"}]], 
       RowBox[{"Flatten", "[", 
        SubscriptBox["y", 
         RowBox[{"d", ",", "1"}]], "]"}]}], "]"}]}], ";"}], 
   "\[IndentingNewLine]", 
   RowBox[{
    RowBox[{
     SubscriptBox["Dy", 
      RowBox[{"d", ",", "2"}]], "=", 
     RowBox[{"Simplify", "[", 
      RowBox[{
       SubscriptBox["\[PartialD]", 
        RowBox[{"{", 
         RowBox[{
          RowBox[{"Flatten", "[", "\[Chi]", "]"}], ",", "1"}], "}"}]], 
       RowBox[{"Flatten", "[", 
        SubscriptBox["y", 
         RowBox[{"d", ",", "2"}]], "]"}]}], "]"}]}], ";"}], "\n", 
   RowBox[{
    RowBox[{
     SubscriptBox["DLfy", 
      RowBox[{"d", ",", "1"}]], "=", 
     RowBox[{
      RowBox[{
       SubscriptBox["\[PartialD]", 
        RowBox[{"{", 
         RowBox[{
          RowBox[{"Flatten", "[", "\[Chi]", "]"}], ",", "1"}], "}"}]], 
       RowBox[{"Flatten", "[", 
        RowBox[{
         SubscriptBox["Dy", 
          RowBox[{"d", ",", "1"}]], ".", "d\[Chi]"}], "]"}]}], "//", 
      "Simplify"}]}], ";"}], "\[IndentingNewLine]", 
   RowBox[{
    RowBox[{
     SubscriptBox["DLfy", 
      RowBox[{"d", ",", "2"}]], "=", 
     RowBox[{
      SubscriptBox["\[PartialD]", 
       RowBox[{"{", 
        RowBox[{
         RowBox[{"Flatten", "[", "\[Chi]", "]"}], ",", "1"}], "}"}]], 
      RowBox[{"Flatten", "[", 
       RowBox[{
        SubscriptBox["Dy", 
         RowBox[{"d", ",", "2"}]], ".", "d\[Chi]"}], "]"}]}]}], ";"}], "\n", 
   RowBox[{"(*", 
    RowBox[{
    "calculate", " ", "actual", " ", "kinematics", " ", "outputs", " ", "and",
      " ", "Jacobians"}], "*)"}], "\n", 
   RowBox[{
    RowBox[{
     SubscriptBox["y", 
      RowBox[{"a", ",", "1"}]], "=", 
     RowBox[{"D", "[", 
      RowBox[{
       SubscriptBox["p", "hip"], ",", "t"}], "]"}]}], ";"}], "\n", 
   RowBox[{
    RowBox[{"NSslopeOutput", " ", "=", " ", 
     RowBox[{"{", 
      RowBox[{"nsslope", ",", "LinearNSslope", ",", "HipAngle"}], "}"}]}], 
    ";"}], "\[IndentingNewLine]", 
   RowBox[{
    RowBox[{"TorsoOutput", " ", "=", " ", 
     RowBox[{"{", 
      RowBox[{
      "TorsoHipAngle", ",", "theta4", ",", "nstorso", ",", "LinearNStorso", 
       ",", "storso", ",", "LinearStorso", ",", 
       SubscriptBox["p", "COM"], ",", 
       SubscriptBox["Linearp", "COM"]}], "}"}]}], ";"}], "\n", 
   RowBox[{
    RowBox[{
     SubscriptBox["y", 
      RowBox[{"a", ",", "2"}]], "=", 
     RowBox[{"Simplify", "[", 
      RowBox[{
       RowBox[{
        RowBox[{"{", 
         RowBox[{
          RowBox[{"{", 
           RowBox[{"NSslopeOutput", "[", 
            RowBox[{"[", "NSslopeIndex", "]"}], "]"}], "}"}], ",", 
          RowBox[{"{", 
           RowBox[{
            SubscriptBox["\[Theta]", "2"], "[", "t", "]"}], "}"}], ",", 
          RowBox[{"{", 
           RowBox[{
            SubscriptBox["\[Theta]", "5"], "[", "t", "]"}], "}"}], ",", 
          RowBox[{"{", 
           RowBox[{"TorsoOutput", " ", "[", 
            RowBox[{"[", "TorsoIndex", "]"}], "]"}], "}"}]}], "}"}], "/.", 
        "constsubs"}], "/.", "p0"}], "]"}]}], ";"}], "\n", 
   RowBox[{
    RowBox[{
     SubscriptBox["Dy", 
      RowBox[{"a", ",", "1"}]], "=", 
     RowBox[{"Simplify", "[", 
      RowBox[{
       SubscriptBox["\[PartialD]", 
        RowBox[{"{", 
         RowBox[{
          RowBox[{"Flatten", "[", "\[Chi]", "]"}], ",", "1"}], "}"}]], 
       RowBox[{"Flatten", "[", 
        SubscriptBox["y", 
         RowBox[{"a", ",", "1"}]], "]"}]}], "]"}]}], ";"}], 
   "\[IndentingNewLine]", 
   RowBox[{
    RowBox[{
     SubscriptBox["Dy", 
      RowBox[{"a", ",", "2"}]], "=", 
     RowBox[{"Simplify", "[", 
      RowBox[{
       SubscriptBox["\[PartialD]", 
        RowBox[{"{", 
         RowBox[{
          RowBox[{"Flatten", "[", "\[Chi]", "]"}], ",", "1"}], "}"}]], 
       RowBox[{"Flatten", "[", 
        SubscriptBox["y", 
         RowBox[{"a", ",", "2"}]], "]"}]}], "]"}]}], ";"}], "\n", 
   RowBox[{
    RowBox[{
     SubscriptBox["DLfy", 
      RowBox[{"a", ",", "1"}]], "=", 
     RowBox[{"Simplify", "[", 
      RowBox[{
       SubscriptBox["\[PartialD]", 
        RowBox[{"{", 
         RowBox[{
          RowBox[{"Flatten", "[", "\[Chi]", "]"}], ",", "1"}], "}"}]], 
       RowBox[{"Flatten", "[", 
        RowBox[{
         SubscriptBox["Dy", 
          RowBox[{"a", ",", "1"}]], ".", "d\[Chi]"}], "]"}]}], "]"}]}], ";"}],
    "\[IndentingNewLine]", 
   RowBox[{
    RowBox[{
     SubscriptBox["DLfy", 
      RowBox[{"a", ",", "2"}]], "=", 
     RowBox[{"Simplify", "[", 
      RowBox[{
       SubscriptBox["\[PartialD]", 
        RowBox[{"{", 
         RowBox[{
          RowBox[{"Flatten", "[", "\[Chi]", "]"}], ",", "1"}], "}"}]], 
       RowBox[{"Flatten", "[", 
        RowBox[{
         SubscriptBox["Dy", 
          RowBox[{"a", ",", "2"}]], ".", "d\[Chi]"}], "]"}]}], "]"}]}], 
    ";"}]}]}]], "Input",
 CellChangeTimes->{
  3.504875918224949*^9, {3.505063952502133*^9, 3.505063953970308*^9}, {
   3.5051489336912956`*^9, 3.5051489340032964`*^9}, 3.513531757509935*^9, {
   3.515870024222504*^9, 3.5158700252255616`*^9}, 3.5158863755610266`*^9, {
   3.5271753048660097`*^9, 3.527175316328361*^9}, {3.5271784037700644`*^9, 
   3.527178409550562*^9}, {3.527194806620797*^9, 3.527194812727977*^9}, {
   3.527194901003693*^9, 3.527194939760748*^9}, 3.527195000990027*^9, 
   3.527195065536849*^9, {3.527197843268285*^9, 3.527197851612855*^9}, {
   3.527266876626479*^9, 3.5272669091021013`*^9}, 3.5272680582479362`*^9, {
   3.52726826837751*^9, 3.5272682685913153`*^9}, {3.527268323380795*^9, 
   3.52726834320557*^9}, {3.527268390320581*^9, 3.527268399102364*^9}, 
   3.5273682847658978`*^9}],

Cell[BoxData[
 RowBox[{"\n", 
  RowBox[{"(*", 
   RowBox[{"Inverse", " ", "Kinematics"}], "*)"}], "\n", 
  RowBox[{"(*", 
   RowBox[{"define", " ", "the", " ", "reset", " ", "map"}], "*)"}], "\n", 
  RowBox[{
   RowBox[{
    RowBox[{"\[CapitalDelta]", "=", " ", 
     RowBox[{"{", 
      RowBox[{
       RowBox[{"{", 
        RowBox[{"1", ",", "1", ",", "1", ",", 
         RowBox[{"-", "1"}], ",", 
         RowBox[{"-", "1"}]}], "}"}], ",", "\[IndentingNewLine]", 
       RowBox[{"{", 
        RowBox[{"0", ",", "0", ",", "0", ",", "0", ",", "1"}], "}"}], ",", 
       "\[IndentingNewLine]", 
       RowBox[{"{", 
        RowBox[{"0", ",", "0", ",", "0", ",", "1", ",", "0"}], "}"}], ",", 
       "\[IndentingNewLine]", 
       RowBox[{"{", 
        RowBox[{"0", ",", "0", ",", "1", ",", "0", ",", "0"}], "}"}], ",", 
       "\[IndentingNewLine]", 
       RowBox[{"{", 
        RowBox[{"0", ",", "1", ",", "0", ",", "0", ",", "0"}], "}"}]}], 
      "}"}]}], ";"}], "\[IndentingNewLine]", 
   RowBox[{"(*", 
    RowBox[{
     RowBox[{
     "define", " ", "the", " ", "guard", " ", "conditions", " ", "for", " ", 
      "pre"}], "-", 
     RowBox[{"and", " ", "post"}], "-", "impact"}], "*)"}], "\n", 
   RowBox[{
    RowBox[{"initialtime", "=", 
     RowBox[{"{", 
      RowBox[{"t", "\[Rule]", "0"}], "}"}]}], ";"}], "\[IndentingNewLine]", 
   RowBox[{"(*", 
    RowBox[{
     RowBox[{
     "compute", " ", "the", " ", "position", " ", "of", " ", "the", " ", 
      "hip", " ", "post"}], "-", "impact"}], "*)"}], "\n", 
   RowBox[{
    RowBox[{"resetmap", "=", 
     RowBox[{"Module", "[", 
      RowBox[{
       RowBox[{"{", "newq", "}"}], ",", "\[IndentingNewLine]", 
       RowBox[{
        RowBox[{"newq", "=", 
         RowBox[{"\[CapitalDelta]", ".", 
          RowBox[{"(", "q", ")"}]}]}], ";", "\[IndentingNewLine]", 
        RowBox[{"Table", "[", 
         RowBox[{
          RowBox[{
           RowBox[{
            SubscriptBox["\[Theta]", "i"], "[", "0", "]"}], "->", 
           RowBox[{"newq", "[", 
            RowBox[{"[", 
             RowBox[{"i", ",", "1"}], "]"}], "]"}]}], ",", 
          RowBox[{"{", 
           RowBox[{"i", ",", "1", ",", 
            RowBox[{"Length", "[", "q", "]"}]}], "}"}]}], "]"}]}]}], 
      "\[IndentingNewLine]", "]"}]}], ";"}], "\n", 
   RowBox[{"(*", 
    RowBox[{
    "Parameterized", " ", "Time", " ", "Using", " ", "the", " ", "Output", 
     " ", "of", " ", "the", " ", "Hip"}], "*)"}], "\[IndentingNewLine]", 
   RowBox[{
    RowBox[{"phipcondition", "=", 
     RowBox[{
      RowBox[{
       RowBox[{"{", 
        RowBox[{
         RowBox[{"p", "[", "1", "]"}], "\[Rule]", 
         SubscriptBox["p", "hip"]}], "}"}], "/.", "constsubs"}], "/.", 
      RowBox[{"t", "\[Rule]", "0"}]}]}], ";"}], "\n", 
   RowBox[{
    RowBox[{"tau", "=", 
     RowBox[{"Simplify", "[", 
      RowBox[{
       RowBox[{
        RowBox[{
         RowBox[{
          RowBox[{"\[Sigma]", "[", "t", "]"}], "/.", "\[InvisibleSpace]", 
          "fb"}], "/.", "\[InvisibleSpace]", "phipcondition"}], "/.", 
        "resetmap"}], "/.", "constsubs"}], "]"}]}], ";"}], "\n", 
   RowBox[{"(*", 
    RowBox[{
     RowBox[{"extract", " ", "the", " ", 
      RowBox[{"(", 
       RowBox[{"relative", " ", "degree", " ", "two"}], ")"}], " ", 
      "position"}], "-", 
     RowBox[{"based", " ", "outputs"}]}], "*)"}], 
   RowBox[{
    RowBox[{
     SubscriptBox["y", "2"], "=", " ", 
     RowBox[{
      RowBox[{
       RowBox[{"(", 
        RowBox[{
         SubscriptBox["y", 
          RowBox[{"a", ",", "2"}]], "  ", "-", " ", 
         SubscriptBox["y", 
          RowBox[{"d", ",", "2"}]]}], "  ", ")"}], "/.", "\[InvisibleSpace]", 
       "phipcondition"}], "/.", "constsubs"}]}], ";"}], "\n", 
   RowBox[{"(*", 
    RowBox[{
     RowBox[{"compute", " ", "the", " ", "pre"}], "-", 
     RowBox[{"and", " ", "post"}], "-", 
     RowBox[{
     "impact", " ", "values", " ", "of", " ", "the", " ", "outputs"}]}], 
    "*)"}], 
   RowBox[{
    RowBox[{"yplus", "=", 
     RowBox[{
      RowBox[{
       RowBox[{
        SubscriptBox["y", "2"], "/.", "initialtime"}], "/.", "resetmap"}], "//",
       "Simplify"}]}], ";"}], "\[IndentingNewLine]", 
   RowBox[{
    RowBox[{"yminus", "=", 
     RowBox[{
      SubscriptBox["y", "2"], "/.", "resetmap"}]}], ";"}], 
   "\[IndentingNewLine]", 
   RowBox[{"(*", 
    RowBox[{
    "solve", " ", "for", " ", "the", " ", "inverse", " ", "kinematics"}], 
    "*)"}], "\n", 
   RowBox[{"(*", 
    RowBox[{
    "create", " ", "a", " ", "function", " ", "to", " ", "solve", " ", "the", 
     " ", "inverse", " ", "kinematics"}], "*)"}], 
   RowBox[{
    RowBox[{
     RowBox[{"SolveEquations", "[", 
      RowBox[{"equations_", ",", "variables_", ",", "solutionnumbers_"}], 
      "]"}], ":=", "\[IndentingNewLine]", 
     RowBox[{"Module", "[", 
      RowBox[{
       RowBox[{"{", 
        RowBox[{"i", ",", 
         RowBox[{"solutions", "=", 
          RowBox[{"{", "}"}]}], ",", "tmpeqn", ",", "tmpsolution"}], "}"}], 
       ",", "\[IndentingNewLine]", 
       RowBox[{
        RowBox[{"For", "[", 
         RowBox[{
          RowBox[{"i", "=", "1"}], ",", 
          RowBox[{"i", "\[LessEqual]", 
           RowBox[{"Length", "[", "variables", "]"}]}], ",", 
          RowBox[{"i", "++"}], ",", "\[IndentingNewLine]", 
          RowBox[{
           RowBox[{"tmpeqn", "=", 
            RowBox[{
             RowBox[{"(", 
              RowBox[{"equations", "[", 
               RowBox[{"[", "i", "]"}], "]"}], ")"}], "\[Equal]", "0"}]}], 
           ";", "\[IndentingNewLine]", 
           RowBox[{"tmpsolution", "=", 
            RowBox[{"{", 
             RowBox[{
              RowBox[{
              "variables", "\[LeftDoubleBracket]", "i", 
               "\[RightDoubleBracket]"}], "\[Rule]", 
              RowBox[{"(", 
               RowBox[{
                RowBox[{
                "variables", "\[LeftDoubleBracket]", "i", 
                 "\[RightDoubleBracket]"}], "/.", 
                RowBox[{"Flatten", "[", 
                 RowBox[{"{", 
                  RowBox[{"solutions", ",", 
                   RowBox[{
                    RowBox[{"Solve", "[", 
                    RowBox[{"tmpeqn", ",", 
                    RowBox[{
                    "variables", "\[LeftDoubleBracket]", "i", 
                    "\[RightDoubleBracket]"}]}], "]"}], 
                    "\[LeftDoubleBracket]", 
                    RowBox[{
                    "solutionnumbers", "\[LeftDoubleBracket]", "i", 
                    "\[RightDoubleBracket]"}], "\[RightDoubleBracket]"}]}], 
                  "}"}], "]"}]}], ")"}]}], "}"}]}], ";", 
           "\[IndentingNewLine]", 
           RowBox[{"solutions", "=", 
            RowBox[{"Join", "[", 
             RowBox[{"solutions", ",", 
              RowBox[{"(", 
               RowBox[{"tmpsolution", "/.", "solutions"}], ")"}]}], 
             "]"}]}]}]}], "\[IndentingNewLine]", "]"}], ";", 
        "\[IndentingNewLine]", "solutions"}]}], "\[IndentingNewLine]", 
      "]"}]}], ";"}], "\n", 
   RowBox[{
    RowBox[{"yhsolve", " ", "=", " ", 
     RowBox[{
      RowBox[{"Join", "[", 
       RowBox[{
        RowBox[{"Flatten", "[", "yplus", "]"}], ",", 
        RowBox[{"{", "h", "}"}]}], "]"}], "/.", 
      RowBox[{"{", 
       RowBox[{
        RowBox[{
         RowBox[{
          SubscriptBox["\[Theta]", "3"], "[", "t", "]"}], "-", 
         RowBox[{
          SubscriptBox["\[Theta]", "4"], "[", "t", "]"}]}], "\[Rule]", " ", 
        RowBox[{"\[Beta]", "[", "t", "]"}]}], "}"}]}]}], ";"}]}]}]], "Input",
 CellChangeTimes->{
  3.504875918224949*^9, {3.505063952502133*^9, 3.505063953970308*^9}, {
   3.5051489336912956`*^9, 3.5051489340032964`*^9}, 3.513531757509935*^9, {
   3.515870024222504*^9, 3.5158700252255616`*^9}, 3.5158863755610266`*^9, {
   3.5271753048660097`*^9, 3.527175316328361*^9}, {3.5271784037700644`*^9, 
   3.527178409550562*^9}, {3.527194806620797*^9, 3.527194812727977*^9}, {
   3.527194901003693*^9, 3.527194939760748*^9}, 3.527195000990027*^9, 
   3.527195065536849*^9, {3.527197843268285*^9, 3.527197851612855*^9}, {
   3.527266876626479*^9, 3.5272669091021013`*^9}, 3.5272680582479362`*^9, {
   3.52726826837751*^9, 3.5272682685913153`*^9}, {3.527268323380795*^9, 
   3.52726834320557*^9}, {3.527268390320581*^9, 3.527268399102364*^9}, 
   3.5273682847658978`*^9}],

Cell[CellGroupData[{

Cell[BoxData[
 RowBox[{
  RowBox[{"(*", 
   RowBox[{
    RowBox[{"MatrixForm", "[", "yhsolve", "]"}], ";"}], "*)"}], 
  "\[IndentingNewLine]", 
  RowBox[{
   RowBox[{"MatrixForm", "[", 
    RowBox[{"yhsolve", "\[LeftDoubleBracket]", 
     RowBox[{"{", 
      RowBox[{"2", ",", "3", ",", "1", ",", "5", ",", "4"}], "}"}], 
     "\[RightDoubleBracket]"}], "]"}], "\n", 
   RowBox[{
    RowBox[{"Theta5Cond", "=", 
     RowBox[{
      RowBox[{"Solve", "[", 
       RowBox[{
        RowBox[{
         RowBox[{
         "yhsolve", "\[LeftDoubleBracket]", "2", "\[RightDoubleBracket]"}], 
         "\[Equal]", "0"}], ",", 
        RowBox[{
         SubscriptBox["\[Theta]", "5"], "[", "t", "]"}]}], "]"}], 
      "\[LeftDoubleBracket]", "1", "\[RightDoubleBracket]"}]}], ";"}], "\n", 
   RowBox[{
    RowBox[{"Theta2Cond", "=", 
     RowBox[{
      RowBox[{"Solve", "[", 
       RowBox[{
        RowBox[{
         RowBox[{
         "yhsolve", "\[LeftDoubleBracket]", "3", "\[RightDoubleBracket]"}], 
         "\[Equal]", "0"}], ",", 
        RowBox[{
         SubscriptBox["\[Theta]", "2"], "[", "t", "]"}]}], "]"}], 
      "\[LeftDoubleBracket]", "1", "\[RightDoubleBracket]"}]}], ";"}], "\n", 
   RowBox[{
    RowBox[{"Theta1Cond", "=", 
     RowBox[{
      RowBox[{"Solve", "[", 
       RowBox[{
        RowBox[{
         RowBox[{
         "yhsolve", "\[LeftDoubleBracket]", "1", "\[RightDoubleBracket]"}], 
         "\[Equal]", "0"}], ",", 
        RowBox[{
         SubscriptBox["\[Theta]", "1"], "[", "t", "]"}]}], "]"}], 
      "\[LeftDoubleBracket]", "1", "\[RightDoubleBracket]"}]}], 
    ";"}]}]}]], "Input",
 CellChangeTimes->{
  3.504875918224949*^9, {3.505063952502133*^9, 3.505063953970308*^9}, {
   3.5051489336912956`*^9, 3.5051489340032964`*^9}, 3.513531757509935*^9, {
   3.515870024222504*^9, 3.5158700252255616`*^9}, 3.5158863755610266`*^9, {
   3.5271753048660097`*^9, 3.527175316328361*^9}, {3.5271784037700644`*^9, 
   3.527178409550562*^9}, {3.527194806620797*^9, 3.527194812727977*^9}, {
   3.527194901003693*^9, 3.527194939760748*^9}, 3.527195000990027*^9, 
   3.527195065536849*^9, {3.527197843268285*^9, 3.527197851612855*^9}, {
   3.527266876626479*^9, 3.5272669091021013`*^9}, 3.5272680582479362`*^9, {
   3.52726826837751*^9, 3.5272682685913153`*^9}, {3.527268323380795*^9, 
   3.52726834320557*^9}, {3.527268390320581*^9, 3.527268445697895*^9}}],

Cell[BoxData[
 FormBox[
  TagBox[
   RowBox[{"(", "\[NoBreak]", 
    TagBox[GridBox[{
       {
        RowBox[{
         RowBox[{"-", 
          RowBox[{"a", "(", 
           RowBox[{"3", ",", "1"}], ")"}]}], "-", 
         RowBox[{"a", "(", 
          RowBox[{"3", ",", "5"}], ")"}], "+", 
         RowBox[{
          SubscriptBox["\[Theta]", "5"], "(", "t", ")"}]}]},
       {
        RowBox[{
         RowBox[{"-", 
          RowBox[{"a", "(", 
           RowBox[{"4", ",", "1"}], ")"}]}], "-", 
         RowBox[{"a", "(", 
          RowBox[{"4", ",", "5"}], ")"}], "+", 
         RowBox[{
          SubscriptBox["\[Theta]", "2"], "(", "t", ")"}]}]},
       {
        RowBox[{
         RowBox[{"-", 
          RowBox[{"a", "(", 
           RowBox[{"2", ",", "1"}], ")"}]}], "-", 
         RowBox[{"a", "(", 
          RowBox[{"2", ",", "5"}], ")"}], "-", 
         FractionBox[
          RowBox[{
           RowBox[{"685", " ", 
            RowBox[{"sin", "(", 
             RowBox[{
              SubscriptBox["\[Theta]", "1"], "(", "t", ")"}], ")"}]}], "+", 
           RowBox[{"514", " ", 
            RowBox[{"sin", "(", 
             RowBox[{
              RowBox[{
               SubscriptBox["\[Theta]", "1"], "(", "t", ")"}], "+", 
              RowBox[{
               SubscriptBox["\[Theta]", "2"], "(", "t", ")"}]}], ")"}]}]}], 
          RowBox[{
           RowBox[{"685", " ", 
            RowBox[{"cos", "(", 
             RowBox[{
              SubscriptBox["\[Theta]", "1"], "(", "t", ")"}], ")"}]}], "+", 
           RowBox[{"514", " ", 
            RowBox[{"cos", "(", 
             RowBox[{
              RowBox[{
               SubscriptBox["\[Theta]", "1"], "(", "t", ")"}], "+", 
              RowBox[{
               SubscriptBox["\[Theta]", "2"], "(", "t", ")"}]}], ")"}]}]}]]}]},
       {
        FractionBox[
         RowBox[{"127", " ", 
          RowBox[{"(", 
           RowBox[{
            RowBox[{"-", 
             RowBox[{"514", " ", 
              RowBox[{"cos", "(", 
               RowBox[{
                RowBox[{"\[Beta]", "(", "t", ")"}], "+", 
                RowBox[{
                 SubscriptBox["\[Theta]", "1"], "(", "t", ")"}], "+", 
                RowBox[{
                 SubscriptBox["\[Theta]", "2"], "(", "t", ")"}]}], ")"}]}]}], 
            "-", 
            RowBox[{"685", " ", 
             RowBox[{"cos", "(", 
              RowBox[{
               RowBox[{"\[Beta]", "(", "t", ")"}], "+", 
               RowBox[{
                SubscriptBox["\[Theta]", "1"], "(", "t", ")"}], "+", 
               RowBox[{
                SubscriptBox["\[Theta]", "2"], "(", "t", ")"}], "-", 
               RowBox[{
                SubscriptBox["\[Theta]", "5"], "(", "t", ")"}]}], ")"}]}], 
            "+", 
            RowBox[{"685", " ", 
             RowBox[{"cos", "(", 
              RowBox[{
               SubscriptBox["\[Theta]", "1"], "(", "t", ")"}], ")"}]}], "+", 
            RowBox[{"514", " ", 
             RowBox[{"cos", "(", 
              RowBox[{
               RowBox[{
                SubscriptBox["\[Theta]", "1"], "(", "t", ")"}], "+", 
               RowBox[{
                SubscriptBox["\[Theta]", "2"], "(", "t", ")"}]}], ")"}]}]}], 
           ")"}]}], "250000"]},
       {
        RowBox[{
         RowBox[{"-", 
          RowBox[{"a", "(", 
           RowBox[{"5", ",", "1"}], ")"}]}], "-", 
         RowBox[{"a", "(", 
          RowBox[{"5", ",", "5"}], ")"}], "+", 
         RowBox[{
          SubscriptBox["\[Theta]", "1"], "(", "t", ")"}], "+", 
         RowBox[{
          SubscriptBox["\[Theta]", "2"], "(", "t", ")"}], "+", 
         RowBox[{
          SubscriptBox["\[Theta]", "3"], "(", "t", ")"}]}]}
      },
      GridBoxAlignment->{
       "Columns" -> {{Center}}, "ColumnsIndexed" -> {}, 
        "Rows" -> {{Baseline}}, "RowsIndexed" -> {}},
      GridBoxSpacings->{"Columns" -> {
          Offset[0.27999999999999997`], {
           Offset[0.5599999999999999]}, 
          Offset[0.27999999999999997`]}, "ColumnsIndexed" -> {}, "Rows" -> {
          Offset[0.2], {
           Offset[0.4]}, 
          Offset[0.2]}, "RowsIndexed" -> {}}],
     Column], "\[NoBreak]", ")"}],
   Function[BoxForm`e$, 
    MatrixForm[BoxForm`e$]]], TraditionalForm]], "Output",
 CellChangeTimes->{3.527268507709256*^9, 3.527281950926869*^9}]
}, Open  ]],

Cell[BoxData[
 RowBox[{
  RowBox[{"ThetabetaCond", "=", 
   RowBox[{
    RowBox[{"Solve", "[", 
     RowBox[{
      RowBox[{
       RowBox[{
       "yhsolve", "\[LeftDoubleBracket]", "5", "\[RightDoubleBracket]"}], 
       "\[Equal]", "0"}], ",", 
      RowBox[{"\[Beta]", "[", "t", "]"}]}], "]"}], "\[LeftDoubleBracket]", 
    "2", "\[RightDoubleBracket]"}]}], ";"}]], "Input",
 CellChangeTimes->{
  3.504875918224949*^9, {3.505063952502133*^9, 3.505063953970308*^9}, {
   3.5051489336912956`*^9, 3.5051489340032964`*^9}, 3.513531757509935*^9, {
   3.515870024222504*^9, 3.5158700252255616`*^9}, 3.5158863755610266`*^9, {
   3.5271753048660097`*^9, 3.527175316328361*^9}, {3.5271784037700644`*^9, 
   3.527178409550562*^9}, {3.527194806620797*^9, 3.527194812727977*^9}, {
   3.527194901003693*^9, 3.527194939760748*^9}, 3.527195000990027*^9, 
   3.527195065536849*^9, {3.527197843268285*^9, 3.527197851612855*^9}, {
   3.527266876626479*^9, 3.5272669091021013`*^9}, 3.5272680582479362`*^9, {
   3.52726826837751*^9, 3.5272682685913153`*^9}, {3.527268323380795*^9, 
   3.52726834320557*^9}, {3.527268390320581*^9, 3.527268455164208*^9}}],

Cell[BoxData[
 RowBox[{
  RowBox[{"Theta3Cond", "=", 
   RowBox[{
    RowBox[{"Solve", "[", 
     RowBox[{
      RowBox[{
       RowBox[{
       "yhsolve", "\[LeftDoubleBracket]", "4", "\[RightDoubleBracket]"}], 
       "\[Equal]", "0"}], ",", 
      RowBox[{
       SubscriptBox["\[Theta]", "3"], "[", "t", "]"}]}], "]"}], 
    "\[LeftDoubleBracket]", "1", "\[RightDoubleBracket]"}]}], ";"}]], "Input",\

 CellChangeTimes->{
  3.504875918224949*^9, {3.505063952502133*^9, 3.505063953970308*^9}, {
   3.5051489336912956`*^9, 3.5051489340032964`*^9}, 3.513531757509935*^9, {
   3.515870024222504*^9, 3.5158700252255616`*^9}, 3.5158863755610266`*^9, {
   3.5271753048660097`*^9, 3.527175316328361*^9}, {3.5271784037700644`*^9, 
   3.527178409550562*^9}, {3.527194806620797*^9, 3.527194812727977*^9}, {
   3.527194901003693*^9, 3.527194939760748*^9}, 3.527195000990027*^9, 
   3.527195065536849*^9, {3.527197843268285*^9, 3.527197851612855*^9}, {
   3.527266876626479*^9, 3.5272669091021013`*^9}, 3.5272680582479362`*^9, {
   3.52726826837751*^9, 3.5272682685913153`*^9}, {3.527268323380795*^9, 
   3.52726834320557*^9}, {3.527268390320581*^9, 3.527268455164208*^9}}],

Cell[BoxData[
 RowBox[{
  RowBox[{"(*", 
   RowBox[{
    RowBox[{
     RowBox[{
      RowBox[{
       RowBox[{
        RowBox[{
         RowBox[{
          RowBox[{
           RowBox[{
            RowBox[{
             RowBox[{
              RowBox[{"Transpose", "[", 
               RowBox[{"{", 
                RowBox[{"{", 
                 RowBox[{
                  RowBox[{
                   SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], ",", 
                  RowBox[{
                   SubscriptBox["\[Theta]", "2"], "[", "t", "]"}], ",", 
                  RowBox[{
                   SubscriptBox["\[Theta]", "3"], "[", "t", "]"}], ",", 
                  RowBox[{
                   RowBox[{
                    SubscriptBox["\[Theta]", "3"], "[", "t", "]"}], "-", 
                   RowBox[{"\[Beta]", "[", "t", "]"}]}], ",", 
                  RowBox[{
                   SubscriptBox["\[Theta]", "5"], "[", "t", "]"}]}], "}"}], 
                "}"}], "]"}], "/.", "\[InvisibleSpace]", "Theta3Cond"}], "/.",
              "\[InvisibleSpace]", "atest"}], "/.", "\[InvisibleSpace]", 
            "ThetabetaCond"}], "/.", "\[InvisibleSpace]", "atest"}], "/.", 
          "\[InvisibleSpace]", "Theta1Cond"}], "/.", "\[InvisibleSpace]", 
         "atest"}], "/.", "\[InvisibleSpace]", "Theta5Cond"}], "/.", 
       "\[InvisibleSpace]", "atest"}], "/.", "\[InvisibleSpace]", 
      "Theta2Cond"}], "/.", "\[InvisibleSpace]", "atest"}], ";"}], "*)"}], 
  "\n", 
  RowBox[{
   RowBox[{
    RowBox[{"Theta1a", " ", "=", " ", 
     RowBox[{
      RowBox[{
       SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "/.", "Theta1Cond"}]}],
     ";"}], "\[IndentingNewLine]", 
   RowBox[{
    RowBox[{"Theta2a", " ", "=", " ", 
     RowBox[{
      RowBox[{
       SubscriptBox["\[Theta]", "2"], "[", "t", "]"}], "/.", "Theta2Cond"}]}],
     ";"}], "\[IndentingNewLine]", 
   RowBox[{
    RowBox[{"Theta3a", " ", "=", " ", 
     RowBox[{
      RowBox[{
       RowBox[{
        SubscriptBox["\[Theta]", "3"], "[", "t", "]"}], "/.", "Theta3Cond"}], 
      "/.", "ThetabetaCond"}]}], ";"}], "\[IndentingNewLine]", 
   RowBox[{
    RowBox[{"Theta4a", " ", "=", " ", 
     RowBox[{
      RowBox[{
       RowBox[{
        RowBox[{
         SubscriptBox["\[Theta]", "3"], "[", "t", "]"}], " ", "-", 
        RowBox[{"\[Beta]", "[", "t", "]"}]}], "/.", "Theta3Cond"}], "/.", 
      "ThetabetaCond"}]}], ";"}], "\[IndentingNewLine]", 
   RowBox[{
    RowBox[{"Theta5a", " ", "=", " ", 
     RowBox[{
      RowBox[{
       SubscriptBox["\[Theta]", "5"], "[", "t", "]"}], "/.", "Theta5Cond"}]}],
     ";"}], "\[IndentingNewLine]", 
   RowBox[{"(*", 
    RowBox[{"Velocity", " ", "of", " ", 
     RowBox[{"Outputs", ":", 
      RowBox[{"Pre", " ", "and", " ", "Post", " ", "Impact"}]}]}], "*)"}], 
   "\n", 
   RowBox[{"(*", 
    RowBox[{
    "compute", " ", "the", " ", "jacobian", " ", "of", " ", "the", " ", 
     "output", " ", "functions"}], "*)"}], 
   RowBox[{
    RowBox[{"Y", "=", 
     RowBox[{"Join", "[", 
      RowBox[{
       RowBox[{
        SubscriptBox["\[PartialD]", 
         RowBox[{"{", 
          RowBox[{
           RowBox[{"Flatten", "[", "q", "]"}], ",", "1"}], "}"}]], 
        RowBox[{"{", 
         SubscriptBox["p", "hip"], "}"}]}], ",", 
       RowBox[{"Simplify", "[", 
        RowBox[{
         SubscriptBox["\[PartialD]", 
          RowBox[{"{", 
           RowBox[{
            RowBox[{"Flatten", "[", "q", "]"}], ",", "1"}], "}"}]], 
         RowBox[{"Flatten", "[", 
          SubscriptBox["y", "2"], "]"}]}], "]"}]}], "]"}]}], ";"}], "\n", 
   RowBox[{
    RowBox[{"Yplus", "=", 
     RowBox[{
      RowBox[{
       RowBox[{"Y", "/.", "initialtime"}], "/.", "constsubs"}], "/.", 
      "resetmap"}]}], ";"}], "\n", 
   RowBox[{
    RowBox[{"Yminus", "=", 
     RowBox[{
      RowBox[{"Y", "/.", "constsubs"}], "/.", "resetmap"}]}], ";"}], "\n", 
   RowBox[{
    RowBox[{"Dh", " ", "=", " ", 
     RowBox[{
      SubscriptBox["\[PartialD]", 
       RowBox[{"{", 
        RowBox[{
         RowBox[{"Flatten", "[", "q", "]"}], ",", "1"}], "}"}]], 
      RowBox[{"{", "h", "}"}]}]}], ";"}]}]}]], "Input",
 CellChangeTimes->{
  3.504875918224949*^9, {3.505063952502133*^9, 3.505063953970308*^9}, {
   3.5051489336912956`*^9, 3.5051489340032964`*^9}, 3.513531757509935*^9, {
   3.515870024222504*^9, 3.5158700252255616`*^9}, 3.5158863755610266`*^9, {
   3.5271753048660097`*^9, 3.527175316328361*^9}, {3.5271784037700644`*^9, 
   3.527178409550562*^9}, {3.527194806620797*^9, 3.527194812727977*^9}, {
   3.527194901003693*^9, 3.527194939760748*^9}, 3.527195000990027*^9, 
   3.527195065536849*^9, {3.527197843268285*^9, 3.527197851612855*^9}, {
   3.527266876626479*^9, 3.5272669091021013`*^9}, 3.5272680582479362`*^9, {
   3.52726826837751*^9, 3.5272682685913153`*^9}, {3.527268323380795*^9, 
   3.52726834320557*^9}, {3.527268390320581*^9, 3.5272684131753683`*^9}}],

Cell[BoxData[{
 RowBox[{
  RowBox[{"SetDirectory", "[", 
   RowBox[{
   "\"\</home/shu/workspace/Research/AMBER_Output_Test/model/\>\"", "<>", 
    "\"\<buildopt_torso\>\""}], "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"stream", "=", 
   RowBox[{"OpenWrite", "[", "\"\<tau\>\"", "]"}]}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Write", "[", 
   RowBox[{"stream", ",", 
    RowBox[{"tau", "/.", "statesubs"}]}], "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Close", "[", "stream", "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{
   RowBox[{"Clear", "[", "stream", "]"}], ";"}], 
  "\[IndentingNewLine]"}], "\[IndentingNewLine]", 
 RowBox[{
  RowBox[{"stream", "=", 
   RowBox[{"OpenWrite", "[", "\"\<y_plus\>\"", "]"}]}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Write", "[", 
   RowBox[{"stream", ",", 
    RowBox[{"yplus", "/.", "statesubs"}]}], "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Close", "[", "stream", "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Clear", "[", "stream", "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"stream", "=", 
   RowBox[{"OpenWrite", "[", "\"\<y_minus\>\"", "]"}]}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Write", "[", 
   RowBox[{"stream", ",", 
    RowBox[{"yminus", "/.", "statesubs"}]}], "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Close", "[", "stream", "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{
   RowBox[{"Clear", "[", "stream", "]"}], ";"}], 
  "\n"}], "\[IndentingNewLine]", 
 RowBox[{
  RowBox[{"stream", "=", 
   RowBox[{"OpenWrite", "[", "\"\<theta_a1\>\"", "]"}]}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Write", "[", 
   RowBox[{"stream", ",", 
    RowBox[{"Theta1a", "/.", "statesubs"}]}], "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Close", "[", "stream", "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Clear", "[", "stream", "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"stream", "=", 
   RowBox[{"OpenWrite", "[", "\"\<theta_a2\>\"", "]"}]}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Write", "[", 
   RowBox[{"stream", ",", 
    RowBox[{"Theta2a", "/.", "statesubs"}]}], "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Close", "[", "stream", "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Clear", "[", "stream", "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"stream", "=", 
   RowBox[{"OpenWrite", "[", "\"\<theta_a3\>\"", "]"}]}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Write", "[", 
   RowBox[{"stream", ",", 
    RowBox[{"Theta3a", "/.", "statesubs"}]}], "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Close", "[", "stream", "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Clear", "[", "stream", "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"stream", "=", 
   RowBox[{"OpenWrite", "[", "\"\<theta_a4\>\"", "]"}]}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Write", "[", 
   RowBox[{"stream", ",", 
    RowBox[{"Theta4a", "/.", "statesubs"}]}], "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Close", "[", "stream", "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Clear", "[", "stream", "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"stream", "=", 
   RowBox[{"OpenWrite", "[", "\"\<theta_a5\>\"", "]"}]}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Write", "[", 
   RowBox[{"stream", ",", 
    RowBox[{"Theta5a", "/.", "statesubs"}]}], "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Close", "[", "stream", "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Clear", "[", "stream", "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"stream", "=", 
   RowBox[{"OpenWrite", "[", "\"\<h_dot_minus\>\"", "]"}]}], 
  ";"}], "\[IndentingNewLine]", 
 RowBox[{
  RowBox[{"Write", "[", 
   RowBox[{"stream", ",", 
    RowBox[{"Dh", "/.", "statesubs"}]}], "]"}], 
  ";"}], "\[IndentingNewLine]", 
 RowBox[{
  RowBox[{"Close", "[", "stream", "]"}], ";"}], "\[IndentingNewLine]", 
 RowBox[{
  RowBox[{"Clear", "[", "stream", "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"stream", "=", 
   RowBox[{"OpenWrite", "[", "\"\<H_plus\>\"", "]"}]}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Write", "[", 
   RowBox[{"stream", ",", 
    RowBox[{
     RowBox[{"Yplus", "/.", "statesubs"}], "/.", "constsubs"}]}], "]"}], 
  ";"}], "\n", 
 RowBox[{
  RowBox[{"Close", "[", "stream", "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Clear", "[", "stream", "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"stream", "=", 
   RowBox[{"OpenWrite", "[", "\"\<H_minus\>\"", "]"}]}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Write", "[", 
   RowBox[{"stream", ",", 
    RowBox[{"Yminus", "/.", "statesubs"}]}], "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Close", "[", "stream", "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Clear", "[", "stream", "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{
   RowBox[{"FolderName", "=", 
    RowBox[{"\"\<buildopt_torso_\>\"", "<>", 
     RowBox[{"ToString", "[", "LineNumber", "]"}]}]}], ";"}], "\n", 
  RowBox[{"(*", 
   RowBox[{
    RowBox[{
    "SetDirectory", "[", 
     "\"\</home/shu/workspace/Research/2DKnee_Torso_Outputs\>\"", "]"}], 
    ";"}], "*)"}]}], "\[IndentingNewLine]", 
 RowBox[{
  RowBox[{
  "SetDirectory", "[", 
   "\"\</home/shu/workspace/Research/AMBER_Output_Test/model/\>\"", "]"}], 
  ";"}], "\n", 
 RowBox[{
  RowBox[{"Run", "[", "\"\<perl math2matopt_torso2.pl\>\"", "]"}], 
  ";"}], "\n", 
 RowBox[{
  RowBox[{"Run", "[", 
   RowBox[{"\"\<mkdir -p \>\"", "<>", "FolderName"}], "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Run", "[", 
   RowBox[{
   "\"\<cp ./buildopt_torso/*.m ./\>\"", "<>", "FolderName", "<>", 
    "\"\</\>\""}], "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"SetDirectory", "[", 
   RowBox[{
   "\"\</home/shu/workspace/Research/AMBER_Output_Test/model/\>\"", "<>", 
    "\"\<build_torso\>\""}], "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"stream", "=", 
   RowBox[{"OpenWrite", "[", "\"\<jpos_mat\>\"", "]"}]}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Write", "[", 
   RowBox[{"stream", ",", 
    RowBox[{"pos", "/.", "statesubs"}]}], "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Close", "[", "stream", "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Clear", "[", "stream", "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"stream", "=", 
   RowBox[{"OpenWrite", "[", "\"\<D_mat\>\"", "]"}]}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Write", "[", 
   RowBox[{"stream", ",", 
    RowBox[{"\[ScriptCapitalD]", "/.", "statesubs"}]}], "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Close", "[", "stream", "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Clear", "[", "stream", "]"}], ";"}]}], "Input",
 CellChangeTimes->{
  3.504875918224949*^9, {3.505063952502133*^9, 3.505063953970308*^9}, {
   3.5051489336912956`*^9, 3.5051489340032964`*^9}, 3.513531757509935*^9, {
   3.515870024222504*^9, 3.5158700252255616`*^9}, 3.5158863755610266`*^9, {
   3.5271753048660097`*^9, 3.527175316328361*^9}, {3.5271784037700644`*^9, 
   3.527178409550562*^9}, {3.527194806620797*^9, 3.527194812727977*^9}, {
   3.527194901003693*^9, 3.527194939760748*^9}, 3.527195000990027*^9, 
   3.527195065536849*^9, {3.527197843268285*^9, 3.527197851612855*^9}, {
   3.527266876626479*^9, 3.5272669091021013`*^9}, 3.5272680582479362`*^9, {
   3.52726826837751*^9, 3.5272682685913153`*^9}, {3.527268323380795*^9, 
   3.52726834320557*^9}, 3.527268390320581*^9, 3.527281377932523*^9, {
   3.5272821034051113`*^9, 3.5272821041342583`*^9}}],

Cell[BoxData[{
 RowBox[{
  RowBox[{"stream", "=", 
   RowBox[{"OpenWrite", "[", "\"\<C_mat\>\"", "]"}]}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Write", "[", 
   RowBox[{"stream", ",", 
    RowBox[{"\[ScriptCapitalC]", "/.", "statesubs"}]}], "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Close", "[", "stream", "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Clear", "[", "stream", "]"}], ";"}]}], "Input",
 CellChangeTimes->{
  3.504875918224949*^9, {3.505063952502133*^9, 3.505063953970308*^9}, {
   3.5051489336912956`*^9, 3.5051489340032964`*^9}, 3.513531757509935*^9, {
   3.515870024222504*^9, 3.5158700252255616`*^9}, 3.5158863755610266`*^9, {
   3.5271753048660097`*^9, 3.527175316328361*^9}, {3.5271784037700644`*^9, 
   3.527178409550562*^9}, {3.527194806620797*^9, 3.527194812727977*^9}, {
   3.527194901003693*^9, 3.527194939760748*^9}, 3.527195000990027*^9, 
   3.527195065536849*^9, {3.527197843268285*^9, 3.527197851612855*^9}, {
   3.527266876626479*^9, 3.5272669091021013`*^9}, 3.5272680582479362`*^9, {
   3.52726826837751*^9, 3.5272682685913153`*^9}, {3.527268323380795*^9, 
   3.52726834320557*^9}, 3.527268390320581*^9, {3.527281377932523*^9, 
   3.52728138118757*^9}, {3.527281421988617*^9, 3.527281425495306*^9}}],

Cell[BoxData[{
 RowBox[{
  RowBox[{"stream", "=", 
   RowBox[{"OpenWrite", "[", "\"\<G_vec\>\"", "]"}]}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Write", "[", 
   RowBox[{"stream", ",", 
    RowBox[{"\[ScriptCapitalG]", "/.", "statesubs"}]}], "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Close", "[", "stream", "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Clear", "[", "stream", "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"stream", "=", 
   RowBox[{"OpenWrite", "[", "\"\<De_mat\>\"", "]"}]}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Write", "[", 
   RowBox[{"stream", ",", 
    RowBox[{"\[ScriptCapitalD]e", "/.", "statesubs"}]}], "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Close", "[", "stream", "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Clear", "[", "stream", "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"stream", "=", 
   RowBox[{"OpenWrite", "[", "\"\<E_mat\>\"", "]"}]}], 
  ";"}], "\[IndentingNewLine]", 
 RowBox[{
  RowBox[{"Write", "[", 
   RowBox[{"stream", ",", 
    RowBox[{"\[ScriptCapitalE]", "/.", "statesubs"}]}], "]"}], 
  ";"}], "\[IndentingNewLine]", 
 RowBox[{
  RowBox[{"Close", "[", "stream", "]"}], ";"}], "\[IndentingNewLine]", 
 RowBox[{
  RowBox[{"Clear", "[", "stream", "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"stream", "=", 
   RowBox[{"OpenWrite", "[", "\"\<h_sca\>\"", "]"}]}], 
  ";"}], "\[IndentingNewLine]", 
 RowBox[{
  RowBox[{"Write", "[", 
   RowBox[{"stream", ",", 
    RowBox[{"h", "/.", "statesubs"}]}], "]"}], ";"}], "\[IndentingNewLine]", 
 RowBox[{
  RowBox[{"Close", "[", "stream", "]"}], ";"}], "\[IndentingNewLine]", 
 RowBox[{
  RowBox[{"Clear", "[", "stream", "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"stream", "=", 
   RowBox[{"OpenWrite", "[", "\"\<h_dot_mat\>\"", "]"}]}], 
  ";"}], "\[IndentingNewLine]", 
 RowBox[{
  RowBox[{"Write", "[", 
   RowBox[{"stream", ",", 
    RowBox[{"hdot", "/.", "statesubs"}]}], "]"}], 
  ";"}], "\[IndentingNewLine]", 
 RowBox[{
  RowBox[{"Close", "[", "stream", "]"}], ";"}], "\[IndentingNewLine]", 
 RowBox[{
  RowBox[{"Clear", "[", "stream", "]"}], ";"}], "\[IndentingNewLine]", 
 RowBox[{
  RowBox[{"stream", "=", 
   RowBox[{"OpenWrite", "[", "\"\<phip_sca\>\"", "]"}]}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Write", "[", 
   RowBox[{"stream", ",", 
    RowBox[{
     RowBox[{
      SubscriptBox["p", "hip"], "/.", "\[InvisibleSpace]", "constsubs"}], "/.",
      "statesubs"}]}], "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Close", "[", "stream", "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{
   RowBox[{"Clear", "[", "stream", "]"}], ";"}], "\n", 
  RowBox[{"(*", 
   RowBox[{
    RowBox[{"stream", "=", 
     RowBox[{"OpenWrite", "[", "\"\<deltaphip_sca\>\"", "]"}]}], ";", "\n", 
    RowBox[{"Write", "[", 
     RowBox[{"stream", ",", 
      RowBox[{
       RowBox[{
        SubscriptBox["deltap", "hip"], "/.", "\[InvisibleSpace]", 
        "constsubs"}], "/.", "statesubs"}]}], "]"}], ";", "\n", 
    RowBox[{"Close", "[", "stream", "]"}], ";", "\n", 
    RowBox[{"Clear", "[", "stream", "]"}], ";"}], "*)"}]}], "\n", 
 RowBox[{
  RowBox[{"stream", "=", 
   RowBox[{"OpenWrite", "[", "\"\<phip_dot_mat\>\"", "]"}]}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Write", "[", 
   RowBox[{"stream", ",", 
    RowBox[{
     RowBox[{
      SubscriptBox["p", "hipdot"], "/.", "\[InvisibleSpace]", "constsubs"}], "/.",
      "statesubs"}]}], "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Close", "[", "stream", "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{
   RowBox[{"Clear", "[", "stream", "]"}], ";"}], "\n", 
  RowBox[{"(*", 
   RowBox[{
    RowBox[{"stream", "=", 
     RowBox[{"OpenWrite", "[", "\"\<deltaphip_dot_mat\>\"", "]"}]}], ";", 
    "\n", 
    RowBox[{"Write", "[", 
     RowBox[{"stream", ",", 
      RowBox[{
       RowBox[{
        SubscriptBox["deltap", "hipdot"], " ", "/.", "\[InvisibleSpace]", 
        "constsubs"}], "/.", "statesubs"}]}], "]"}], ";", "\n", 
    RowBox[{"Close", "[", "stream", "]"}], ";", "\n", 
    RowBox[{"Clear", "[", "stream", "]"}], ";"}], "*)"}]}], "\n", 
 RowBox[{
  RowBox[{"stream", "=", 
   RowBox[{"OpenWrite", "[", "\"\<yd1_sca\>\"", "]"}]}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Write", "[", 
   RowBox[{"stream", ",", 
    RowBox[{
     RowBox[{
      SubscriptBox["y", 
       RowBox[{"d", ",", "1"}]], "/.", "\[InvisibleSpace]", "constsubs"}], "/.",
      "statesubs"}]}], "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Close", "[", "stream", "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Clear", "[", "stream", "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"stream", "=", 
   RowBox[{"OpenWrite", "[", "\"\<yd2_vec\>\"", "]"}]}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Write", "[", 
   RowBox[{"stream", ",", 
    RowBox[{
     RowBox[{
      SubscriptBox["y", 
       RowBox[{"d", ",", "2"}]], "/.", "\[InvisibleSpace]", "constsubs"}], "/.",
      "statesubs"}]}], "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Close", "[", "stream", "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Clear", "[", "stream", "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"stream", "=", 
   RowBox[{"OpenWrite", "[", "\"\<Dyd1_mat\>\"", "]"}]}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Write", "[", 
   RowBox[{"stream", ",", 
    RowBox[{
     RowBox[{
      SubscriptBox["Dy", 
       RowBox[{"d", ",", "1"}]], "/.", "\[InvisibleSpace]", "constsubs"}], "/.",
      "statesubs"}]}], "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Close", "[", "stream", "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Clear", "[", "stream", "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"stream", "=", 
   RowBox[{"OpenWrite", "[", "\"\<Dyd2_mat\>\"", "]"}]}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Write", "[", 
   RowBox[{"stream", ",", 
    RowBox[{
     RowBox[{
      SubscriptBox["Dy", 
       RowBox[{"d", ",", "2"}]], "/.", "\[InvisibleSpace]", "constsubs"}], "/.",
      "statesubs"}]}], "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Close", "[", "stream", "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Clear", "[", "stream", "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"stream", "=", 
   RowBox[{"OpenWrite", "[", "\"\<DLfyd1_mat\>\"", "]"}]}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Write", "[", 
   RowBox[{"stream", ",", 
    RowBox[{
     RowBox[{
      SubscriptBox["DLfy", 
       RowBox[{"d", ",", "1"}]], "/.", "\[InvisibleSpace]", "constsubs"}], "/.",
      "statesubs"}]}], "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Close", "[", "stream", "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Clear", "[", "stream", "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"stream", "=", 
   RowBox[{"OpenWrite", "[", "\"\<DLfyd2_mat\>\"", "]"}]}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Write", "[", 
   RowBox[{"stream", ",", 
    RowBox[{
     RowBox[{
      SubscriptBox["DLfy", 
       RowBox[{"d", ",", "2"}]], "/.", "\[InvisibleSpace]", "constsubs"}], "/.",
      "statesubs"}]}], "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Close", "[", "stream", "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Clear", "[", "stream", "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"stream", "=", 
   RowBox[{"OpenWrite", "[", "\"\<ya1_sca\>\"", "]"}]}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Write", "[", 
   RowBox[{"stream", ",", 
    RowBox[{
     RowBox[{
      SubscriptBox["y", 
       RowBox[{"a", ",", "1"}]], "/.", "\[InvisibleSpace]", "constsubs"}], "/.",
      "statesubs"}]}], "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Close", "[", "stream", "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Clear", "[", "stream", "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"stream", "=", 
   RowBox[{"OpenWrite", "[", "\"\<ya2_vec\>\"", "]"}]}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Write", "[", 
   RowBox[{"stream", ",", 
    RowBox[{
     RowBox[{
      SubscriptBox["y", 
       RowBox[{"a", ",", "2"}]], "/.", "\[InvisibleSpace]", "constsubs"}], "/.",
      "statesubs"}]}], "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Close", "[", "stream", "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Clear", "[", "stream", "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"stream", "=", 
   RowBox[{"OpenWrite", "[", "\"\<Dya1_mat\>\"", "]"}]}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Write", "[", 
   RowBox[{"stream", ",", 
    RowBox[{
     RowBox[{
      SubscriptBox["Dy", 
       RowBox[{"a", ",", "1"}]], "/.", "\[InvisibleSpace]", "constsubs"}], "/.",
      "statesubs"}]}], "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Close", "[", "stream", "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Clear", "[", "stream", "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"stream", "=", 
   RowBox[{"OpenWrite", "[", "\"\<Dya2_mat\>\"", "]"}]}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Write", "[", 
   RowBox[{"stream", ",", 
    RowBox[{
     RowBox[{
      SubscriptBox["Dy", 
       RowBox[{"a", ",", "2"}]], "/.", "\[InvisibleSpace]", "constsubs"}], "/.",
      "statesubs"}]}], "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Close", "[", "stream", "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Clear", "[", "stream", "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"stream", "=", 
   RowBox[{"OpenWrite", "[", "\"\<DLfya1_mat\>\"", "]"}]}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Write", "[", 
   RowBox[{"stream", ",", 
    RowBox[{
     RowBox[{
      SubscriptBox["DLfy", 
       RowBox[{"a", ",", "1"}]], "/.", "\[InvisibleSpace]", "constsubs"}], "/.",
      "statesubs"}]}], "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Close", "[", "stream", "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Clear", "[", "stream", "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"stream", "=", 
   RowBox[{"OpenWrite", "[", "\"\<DLfya2_mat\>\"", "]"}]}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Write", "[", 
   RowBox[{"stream", ",", 
    RowBox[{
     RowBox[{
      SubscriptBox["DLfy", 
       RowBox[{"a", ",", "2"}]], "/.", "\[InvisibleSpace]", "constsubs"}], "/.",
      "statesubs"}]}], "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Close", "[", "stream", "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Clear", "[", "stream", "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"stream", "=", 
   RowBox[{"OpenWrite", "[", "\"\<sigma_sca\>\"", "]"}]}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Write", "[", 
   RowBox[{"stream", ",", 
    RowBox[{
     RowBox[{"\[Sigma]y", "/.", "\[InvisibleSpace]", "constsubs"}], "/.", 
     "statesubs"}]}], "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Close", "[", "stream", "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Clear", "[", "stream", "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{
   RowBox[{"FolderName", "=", 
    RowBox[{"\"\<build_torso_\>\"", "<>", 
     RowBox[{"ToString", "[", "LineNumber", "]"}]}]}], ";"}], "\n", 
  RowBox[{"(*", 
   RowBox[{
    RowBox[{
    "SetDirectory", "[", 
     "\"\</home/shu/workspace/Research/2DKnee_Torso_Outputs\>\"", "]"}], 
    ";"}], "*)"}]}], "\[IndentingNewLine]", 
 RowBox[{
  RowBox[{
  "SetDirectory", "[", 
   "\"\</home/shu/workspace/Research/AMBER_Output_Test/model/\>\"", "]"}], 
  ";"}], "\n", 
 RowBox[{
  RowBox[{"Run", "[", "\"\<perl math2mat_torso.pl\>\"", "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Run", "[", 
   RowBox[{"\"\<mkdir -p \>\"", "<>", "FolderName"}], "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Run", "[", 
   RowBox[{
   "\"\<cp ./build_torso/*.m ./\>\"", "<>", "FolderName", "<>", "\"\</\>\""}],
    "]"}], ";"}]}], "Input",
 CellChangeTimes->{
  3.504875918224949*^9, {3.505063952502133*^9, 3.505063953970308*^9}, {
   3.5051489336912956`*^9, 3.5051489340032964`*^9}, 3.513531757509935*^9, {
   3.515870024222504*^9, 3.5158700252255616`*^9}, 3.5158863755610266`*^9, {
   3.5271753048660097`*^9, 3.527175316328361*^9}, {3.5271784037700644`*^9, 
   3.527178409550562*^9}, {3.527194806620797*^9, 3.527194812727977*^9}, {
   3.527194901003693*^9, 3.527194939760748*^9}, 3.527195000990027*^9, 
   3.527195065536849*^9, {3.527197843268285*^9, 3.527197851612855*^9}, {
   3.527266876626479*^9, 3.5272669091021013`*^9}, 3.5272680582479362`*^9, {
   3.52726826837751*^9, 3.5272682685913153`*^9}, {3.527268323380795*^9, 
   3.52726834320557*^9}, 3.527268390320581*^9, {3.527281377932523*^9, 
   3.52728138118757*^9}, {3.5272820839143476`*^9, 3.527282084980423*^9}}]
},
WindowSize->{959, 1026},
WindowMargins->{{-10, Automatic}, {Automatic, -8}},
ShowSelection->True,
Magnification->1.100000023841858,
FrontEndVersion->"8.0 for Linux x86 (64-bit) (November 7, 2010)",
StyleDefinitions->"Default.nb"
]
(* End of Notebook Content *)

(* Internal cache information *)
(*CellTagsOutline
CellTagsIndex->{}
*)
(*CellTagsIndex
CellTagsIndex->{}
*)
(*NotebookFileOutline
Notebook[{
Cell[CellGroupData[{
Cell[567, 22, 2609, 64, 279, "Input"],
Cell[3179, 88, 171, 3, 31, "Output"],
Cell[3353, 93, 169, 3, 31, "Output"],
Cell[3525, 98, 169, 3, 31, "Output"]
}, Open  ]],
Cell[3709, 104, 2098, 51, 132, "Input"],
Cell[5810, 157, 14198, 433, 1009, "Input"],
Cell[20011, 592, 8148, 239, 213, "Input"],
Cell[CellGroupData[{
Cell[28184, 835, 7267, 201, 271, "Input"],
Cell[35454, 1038, 1750, 53, 120, "Output"],
Cell[37207, 1093, 3252, 93, 120, "Output"],
Cell[40462, 1188, 4476, 124, 120, "Output"],
Cell[44941, 1314, 5098, 138, 120, "Output"]
}, Open  ]],
Cell[50054, 1455, 2996, 81, 127, "Input"],
Cell[CellGroupData[{
Cell[53075, 1540, 7676, 220, 241, "Input"],
Cell[60754, 1762, 10297, 287, 73, "Output"]
}, Open  ]],
Cell[CellGroupData[{
Cell[71088, 2054, 87, 2, 31, "Input"],
Cell[71178, 2058, 3525, 100, 111, "Output"]
}, Open  ]],
Cell[CellGroupData[{
Cell[74740, 2163, 150, 3, 31, "Input"],
Cell[74893, 2168, 2681, 78, 111, "Output"]
}, Open  ]],
Cell[CellGroupData[{
Cell[77611, 2251, 8057, 216, 506, "Input"],
Cell[85671, 2469, 2859, 80, 92, "Output"],
Cell[88533, 2551, 1049, 26, 51, "Output"]
}, Open  ]],
Cell[CellGroupData[{
Cell[89619, 2582, 43, 0, 31, "Input"],
Cell[89665, 2584, 115006, 2844, 172, "Output"]
}, Open  ]],
Cell[CellGroupData[{
Cell[204708, 5433, 10249, 301, 727, "Input"],
Cell[214960, 5736, 4145, 118, 172, "Output"]
}, Open  ]],
Cell[219120, 5857, 761, 11, 31, "Input"],
Cell[219884, 5870, 9575, 287, 672, "Input"],
Cell[229462, 6159, 8331, 218, 716, "Input"],
Cell[CellGroupData[{
Cell[237818, 6381, 2371, 59, 108, "Input"],
Cell[240192, 6442, 4318, 120, 133, "Output"]
}, Open  ]],
Cell[244525, 6565, 1137, 22, 31, "Input"],
Cell[245665, 6589, 1164, 24, 31, "Input"],
Cell[246832, 6615, 4890, 129, 262, "Input"],
Cell[251725, 6746, 6979, 200, 1191, "Input"],
Cell[258707, 6948, 1222, 23, 89, "Input"],
Cell[259932, 6973, 11790, 346, 1809, "Input"]
}
]
*)

(* End of internal cache information *)
